diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index f0c5b0c..f8eb76d 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -458,6 +458,7 @@ public class RobotContainer {
     maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
 
     ArrayList<Command> commands = new ArrayList<Command>();
+    commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
 
     PIDController xController = SwerveDriveConstants.X_CONTROLLER;
     PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
@@ -503,6 +504,15 @@ public class RobotContainer {
    */
   public Command getAutonomousCommand() {
 
+    // ! PathPlanner Testing
+    if (!this.currentClimberMode.equals(null)) {
+      return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Drive Forward"));
+    }
+
+    if (!this.currentClimberMode.equals(null)) {
+      return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond"));
+    }
+
     // ! ways to not coast
     // // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive);
     // * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive);
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 43eb4f8..c21c45c 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -75,12 +75,7 @@ public class SwerveDrive extends SubsystemBase {
     //     VecBuilder.fill(Units.degreesToRadians(1)),            // TODO: tune
     //     VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
     
-    m_odometry = new SwerveDriveOdometry(
-        m_kinematics,
-        getRegGyro(),//m_gyro.getRotation2d(),
-        new Pose2d()); // TODO: tune
-
-    // m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
+    m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
 
     m_gyro.reset();
     SmartDashboard.putData("Field", m_field);
@@ -192,9 +187,9 @@ public class SwerveDrive extends SubsystemBase {
 
     // chassis speeds
     // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
-    SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
-    SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
-    SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
   }
 
   /**
@@ -246,11 +241,11 @@ public class SwerveDrive extends SubsystemBase {
     SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
     SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
 
-    m_odometry.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
-                            modules[0].getState(),
-                            modules[1].getState(),
-                            modules[2].getState(),
-                            modules[3].getState());
+    m_odometry.update( m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
+                       modules[0].getState(),
+                       modules[1].getState(),
+                       modules[2].getState(),
+                       modules[3].getState());
   }