From b17ab324e4f5dc64a401f64c820f6320e473c313 Mon Sep 17 00:00:00 2001
From: nathanrsxtn <nathanrsxtn@gmail.com>
Date: Tue, 22 Mar 2022 18:18:04 -0600
Subject: [PATCH] Quick switch to swerve odometry

---
 .../frc4388/robot/commands/PathRecorder.java  |  2 +-
 .../frc4388/robot/subsystems/SwerveDrive.java | 27 ++++++++++++-------
 .../robot/subsystems/VisionOdometry.java      | 18 ++++++-------
 3 files changed, 27 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/PathRecorder.java b/src/main/java/frc4388/robot/commands/PathRecorder.java
index 0964f33..17f0e58 100644
--- a/src/main/java/frc4388/robot/commands/PathRecorder.java
+++ b/src/main/java/frc4388/robot/commands/PathRecorder.java
@@ -105,7 +105,7 @@ public class PathRecorder extends CommandBase {
    */
   @Override
   public void execute() {
-    Translation2d position = m_swerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
+    Translation2d position = m_swerveDrive.m_odometry.getPoseMeters().getTranslation();
     Rotation2d rotation = m_swerveDrive.m_gyro.getRotation2d();
     // FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity.
     Translation2d velocity = new Translation2d(m_swerveDrive.getChassisSpeeds().vxMetersPerSecond, m_swerveDrive.getChassisSpeeds().vyMetersPerSecond);
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 9c0429f..997d08f 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -13,6 +13,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -45,7 +46,7 @@ public class SwerveDrive extends SubsystemBase {
   public SwerveModule[] modules;
   public WPI_Pigeon2 m_gyro;
 
-  public SwerveDrivePoseEstimator m_poseEstimator;
+  public SwerveDriveOdometry m_odometry;
   // public SwerveDriveOdometry m_odometry;
 
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
@@ -66,13 +67,18 @@ public class SwerveDrive extends SubsystemBase {
 
     modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
     
-    m_poseEstimator = new SwerveDrivePoseEstimator(
-        getRegGyro(),//m_gyro.getRotation2d(),
-        new Pose2d(),
+    // m_poseEstimator = new SwerveDrivePoseEstimator(
+    //     getRegGyro(),//m_gyro.getRotation2d(),
+    //     new Pose2d(),
+    //     m_kinematics,
+    //     VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),  // TODO: tune
+    //     VecBuilder.fill(Units.degreesToRadians(1)),            // TODO: tune
+    //     VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
+    
+    m_odometry = new SwerveDriveOdometry(
         m_kinematics,
-        VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),  // TODO: tune
-        VecBuilder.fill(Units.degreesToRadians(1)),            // TODO: tune
-        VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
+        getRegGyro(),//m_gyro.getRotation2d(),
+        new Pose2d()); // TODO: tune
 
     // m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
 
@@ -206,7 +212,8 @@ public class SwerveDrive extends SubsystemBase {
    */
   public Pose2d getOdometry() {
     // return m_odometry.getPoseMeters();
-    return m_poseEstimator.getEstimatedPosition();
+    return m_odometry.getPoseMeters();
+    // return m_poseEstimator.getEstimatedPosition();
   }
 
   /**
@@ -226,7 +233,7 @@ public class SwerveDrive extends SubsystemBase {
    * Resets the odometry of the robot to the given pose.
    */
   public void resetOdometry(Pose2d pose) {
-    m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
+    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
   }
 
   /**
@@ -239,7 +246,7 @@ public class SwerveDrive extends SubsystemBase {
     SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
     SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
 
-    m_poseEstimator.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
+    m_odometry.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
                             modules[0].getState(),
                             modules[1].getState(),
                             modules[2].getState(),
diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
index 0f8f0bd..4711063 100644
--- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
@@ -146,15 +146,15 @@ public class VisionOdometry extends SubsystemBase {
     return m_camera.getLEDMode() != VisionLEDMode.kOff;
   }
 
-  public void updateOdometryWithVision(){
-    try {
-      m_drive.m_poseEstimator.addVisionMeasurement(
-              getVisionOdometry(),
-              Timer.getFPGATimestamp() - getLatency());
-    } catch (VisionObscuredException err) {
-      err.printStackTrace();
-    } 
-  }
+  // public void updateOdometryWithVision(){
+  //   try {
+  //     m_drive.m_poseEstimator.addVisionMeasurement(
+  //             getVisionOdometry(),
+  //             Timer.getFPGATimestamp() - getLatency());
+  //   } catch (VisionObscuredException err) {
+  //     err.printStackTrace();
+  //   } 
+  // }
 
   /** Reverse 3d projects target points from screen coordinates to 3d space
    * <p>