expose pose2d with the 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

@@ -7,6 +7,8 @@
 
 package frc.robot.subsystems.swerve;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.ctre.phoenix6.Utils;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -54,50 +56,47 @@ public SwerveTelemetry(double maxSpeed) {
   private double lastTime = Utils.getCurrentTimeSeconds();
 
   /* Mechanisms to represent the swerve module states */
-  private final Mechanism2d[] m_moduleMechanisms =
-      new Mechanism2d[] {
-        new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1),
-      };
+  private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
+      new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1),
+  };
   /* A direction and length changing ligament for speed representation */
-  private final MechanismLigament2d[] m_moduleSpeeds =
-      new MechanismLigament2d[] {
-        m_moduleMechanisms[0]
-            .getRoot("RootSpeed", 0.5, 0.5)
-            .append(new MechanismLigament2d("Speed", 0.5, 0)),
-        m_moduleMechanisms[1]
-            .getRoot("RootSpeed", 0.5, 0.5)
-            .append(new MechanismLigament2d("Speed", 0.5, 0)),
-        m_moduleMechanisms[2]
-            .getRoot("RootSpeed", 0.5, 0.5)
-            .append(new MechanismLigament2d("Speed", 0.5, 0)),
-        m_moduleMechanisms[3]
-            .getRoot("RootSpeed", 0.5, 0.5)
-            .append(new MechanismLigament2d("Speed", 0.5, 0)),
-      };
+  private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
+      m_moduleMechanisms[0]
+          .getRoot("RootSpeed", 0.5, 0.5)
+          .append(new MechanismLigament2d("Speed", 0.5, 0)),
+      m_moduleMechanisms[1]
+          .getRoot("RootSpeed", 0.5, 0.5)
+          .append(new MechanismLigament2d("Speed", 0.5, 0)),
+      m_moduleMechanisms[2]
+          .getRoot("RootSpeed", 0.5, 0.5)
+          .append(new MechanismLigament2d("Speed", 0.5, 0)),
+      m_moduleMechanisms[3]
+          .getRoot("RootSpeed", 0.5, 0.5)
+          .append(new MechanismLigament2d("Speed", 0.5, 0)),
+  };
   /* A direction changing and length constant ligament for module direction */
-  private final MechanismLigament2d[] m_moduleDirections =
-      new MechanismLigament2d[] {
-        m_moduleMechanisms[0]
-            .getRoot("RootDirection", 0.5, 0.5)
-            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
-        m_moduleMechanisms[1]
-            .getRoot("RootDirection", 0.5, 0.5)
-            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
-        m_moduleMechanisms[2]
-            .getRoot("RootDirection", 0.5, 0.5)
-            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
-        m_moduleMechanisms[3]
-            .getRoot("RootDirection", 0.5, 0.5)
-            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
-      };
+  private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
+      m_moduleMechanisms[0]
+          .getRoot("RootDirection", 0.5, 0.5)
+          .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+      m_moduleMechanisms[1]
+          .getRoot("RootDirection", 0.5, 0.5)
+          .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+      m_moduleMechanisms[2]
+          .getRoot("RootDirection", 0.5, 0.5)
+          .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+      m_moduleMechanisms[3]
+          .getRoot("RootDirection", 0.5, 0.5)
+          .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+  };
 
   /* Accept the swerve drive state and telemeterize it to smartdashboard */
   public void telemeterize(SwerveDriveState state) {
     /* Telemeterize the pose */
     Pose2d pose = state.Pose;
     fieldTypePub.set("Field2d");
-    fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()});
-
+    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;