Adding necessary code

src/main/java/frc/robot/RobotContainer.java

@@ -5,34 +5,42 @@
 package frc.robot;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.utilities.PoseSendable;
 
 /*
- * The objective for this part of testing is to just make sure swerve is working in a perfect world scenario.
- * This is designed to be as close the REV template as possible, the only difference being the naming of certain 
- * Constants and the positioning of instance variables and there declarations. 
+ * The objective for this part of testing is to see if we can get pose estimation values to work for the gyro angle
+ * required for Field Oriented AND can we find a way to reset the robot's pose to properly offset using the pose estimator. 
  * 
  * We should try to solve in this branch:
- * - Does Field Oriented Drive work in a perfect world scenario?
- * - Is our weird module spin behavior still present?
- *  - If it is, can we do something about it?
+ * - Does Field Oriented Drive work in a perfect world scenario when using the pose estimator as the rotation source?
+ * - Can we reset our pose in such a way using the pose estimator that we get consistent behavior of field oriented drive
+ *      regardless of what orientation the robot is booted up in?
  * 
- * Any changes we make should be able to be carried forward to part 2
+ * Any changes we make should be able to be carried forward to part 3
  */
 public class RobotContainer {
     private Drivetrain drivetrain;
 
     private CommandXboxController driver;
 
+    private PoseSendable poseSendable;
+
     public RobotContainer() {
         drivetrain = new Drivetrain(new Pose2d());
 
         driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
 
+        poseSendable = new PoseSendable();
+
         configureBindings();
+        shuffleboardConfig();
     }
 
     private void configureBindings() {
@@ -46,10 +54,25 @@ public class RobotContainer {
         );
 
         driver.a().onTrue(drivetrain.zeroHeadingCommand());
-        
-  }
 
-  public Command getAutonomousCommand() {
-        return null;
-  }
+        driver.b().onTrue(
+            new InstantCommand(() -> {
+                System.out.println("X: " + poseSendable.getX());
+                System.out.println("Y: " + poseSendable.getY());
+                System.out.println("Rotation: " + poseSendable.getRotation());
+                drivetrain.resetOdometry(poseSendable.getPose());
+            }
+        ));
+    }
+
+    private void shuffleboardConfig() {
+        ShuffleboardTab tab = Shuffleboard.getTab("Testing");
+        tab.add("Drivetrain Pose", poseSendable)
+            .withPosition(0, 0)
+            .withSize(2, 3);
+    }
+
+    public Command getAutonomousCommand() {
+            return null;
+    }
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -208,7 +208,7 @@ public class Drivetrain extends SubsystemBase {
 
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative
-                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle()))
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_poseEstimator.getEstimatedPosition().getRotation())
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);

src/main/java/frc/robot/utilities/PoseSendable.java

@@ -0,0 +1,62 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+public class PoseSendable implements Sendable {
+
+    private double x;
+    private double y;
+    private double rot;
+
+    public PoseSendable() {
+        this(0, 0, 0);
+    }
+
+    public PoseSendable(double x, double y, double rot) {
+        this.x = x;
+        this.y = y;
+        this.rot = rot;
+    }
+
+    public double getX() {
+        return x;
+    }
+
+    public double getY() {
+        return y;
+    }
+
+    public double getRotation() {
+        return rot;
+    }
+
+    public Rotation2d getRotation2d() {
+        return Rotation2d.fromDegrees(rot);
+    }
+
+    public Pose2d getPose() {
+        return new Pose2d(x, y, getRotation2d());
+    }
+
+    public void setX(double x) {
+        this.x = x;
+    }
+
+    public void setY(double y) {
+        this.y = y;
+    }
+
+    public void setRotation(double rot) {
+        this.rot = rot;
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        builder.addDoubleProperty("X", this::getX, this::setX);
+        builder.addDoubleProperty("Y", this::getY, this::setY);
+        builder.addDoubleProperty("Rotation", this::getRotation, this::setRotation);
+    }
+}