Adding necessary code

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,17 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "Shuffleboard": {
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

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);
+    }
+}