From 60763f00cc77d3e7e665d89a0ea4210a040c3bd5 Mon Sep 17 00:00:00 2001
From: Andrew Chen <bobopop787@gmail.com>
Date: Sat, 17 Feb 2024 17:32:33 -0500
Subject: [PATCH] Attempt at swerve odometry continue:

- use pose estimator wrapper for limelight later
- ensure offset is used in swerve position getter
---
 .../subsystems/swerve/SDSSwerveModule.java    |  3 ++-
 .../robot/subsystems/swerve/SwerveDrive.java  | 24 +++++++++++++++++--
 2 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/swerve/SDSSwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SDSSwerveModule.java
index da0e293..3f91422 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SDSSwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SDSSwerveModule.java
@@ -86,6 +86,7 @@ public SDSSwerveModule(String name, int turnID, int driveID, Rotation2d angularO
         driveMotor.setSmartCurrentLimit(20);
         driveEncoder.setPositionConversionFactor(SwerveConstants.kDrivePositionConversionFactor);
         driveEncoder.setVelocityConversionFactor(SwerveConstants.kDriveVelocityConversionFactor);
+        driveEncoder.setPositionConversionFactor(1); // TEMPORARY JUST TO CHECK IF ODOMETRY GOES THE RIGHT WAY
 
         turnPIDController.setPositionPIDWrappingEnabled(true);
         turnPIDController.setPositionPIDWrappingMinInput(0);
@@ -148,7 +149,7 @@ public SwerveModuleState getDesiredSwerveState() {
     public SwerveModulePosition getPosition() {
         return new SwerveModulePosition( 
             driveEncoder.getPosition(),
-            useAbsolute ? Rotation2d.fromRadians(getAbsTurnPos()) : Rotation2d.fromRadians(getRelTurnPos() / 2)
+            useAbsolute ? Rotation2d.fromRadians(getAbsTurnPos()).plus(chassisAngularOffset) : Rotation2d.fromRadians(getRelTurnPos() / 2)
         );
     }
 
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
index f7b3630..7c28720 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
@@ -6,11 +6,14 @@
 import com.ctre.phoenix6.hardware.Pigeon2;
 
 import edu.wpi.first.math.MathUtil;
+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;
 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.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.Preferences;
@@ -36,7 +39,7 @@ public class SwerveDrive extends SubsystemBase {
     private Pigeon2 gyro;
 
     private SwerveDriveKinematics swerveKinematics;
-    private SwerveDriveOdometry swerveOdomentry;
+    private SwerveDrivePoseEstimator swerveOdomentry;
 
     private RateLimiter magnitudeAccelLimiter;
     private RateLimiter directionVelLimiter;
@@ -108,7 +111,12 @@ private void initMathModels() {
             new Translation2d(SwerveConstants.kWheelDistanceMeters / 2, -SwerveConstants.kWheelDistanceMeters / 2),
             new Translation2d(SwerveConstants.kWheelDistanceMeters / 2, SwerveConstants.kWheelDistanceMeters / 2)
         );
-        // swerveOdomentry = new SwerveDriveOdometry(m_swerveKinematics, Rotation2d.fromDegrees(m_gyro.getAngle()), null);
+        swerveOdomentry = new SwerveDrivePoseEstimator(swerveKinematics, Rotation2d.fromDegrees(gyro.getAngle()), new SwerveModulePosition[]{
+            backLeftModule.getPosition(),
+            backRightModule.getPosition(),
+            frontLeftModule.getPosition(),
+            frontRightModule.getPosition()
+        }, new Pose2d());
     }
 
     private void initSimulations() {
@@ -141,6 +149,8 @@ private void initSimulations() {
 
         field = new Field2d();
         
+        field.setRobotPose(new Pose2d(1, 1, new Rotation2d(0)));
+        
         SmartDashboard.putData("Swerve Visualization", swerveVisualizer);
         SmartDashboard.putData("Field", field);
         
@@ -283,5 +293,15 @@ public void periodic() {
         backRightModule.putInfo("backRight");
 
         SmartDashboard.putNumber("Gyro", gyro.getAngle());
+
+        swerveOdomentry.update(Rotation2d.fromDegrees(gyro.getAngle()), new SwerveModulePosition[]{
+            backLeftModule.getPosition(),
+            backRightModule.getPosition(),
+            frontLeftModule.getPosition(),
+            frontRightModule.getPosition()
+        });
+        field.setRobotPose(swerveOdomentry.getEstimatedPosition());
+        System.out.println(swerveOdomentry.getEstimatedPosition().getX() + " " + swerveOdomentry.getEstimatedPosition().getY());
+        SmartDashboard.putData(field);
     }
 }