SwerveTelemetry: publish Pose2D & Field2D

simgui.json

@@ -57,6 +57,17 @@
       "/SmartDashboard/Sim Field": "Field2d"
     },
     "windows": {
+      "/Pose": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      },
       "/Shuffleboard/Intake/Intake Motor": {
         "window": {
           "visible": true

src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java

@@ -21,6 +21,7 @@
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.robot.subsystems.swerve.kit.KitSwerveDrivetrain.SwerveDriveState;
+import org.littletonrobotics.junction.Logger;
 
 public class SwerveTelemetry {
   private final double MaxSpeed;
@@ -97,7 +98,7 @@ public void telemeterize(SwerveDriveState state) {
     Pose2d pose = state.Pose;
     fieldTypePub.set("Field2d");
     fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()});
-
+    Logger.recordOutput("SwervePoseCalculated", pose);
     /* Telemeterize the robot's general speeds */
     double currentTime = Utils.getCurrentTimeSeconds();
     double diffTime = currentTime - lastTime;