Merge branch 'main' into autogen-controller-map

Team3256 · Oct 7, 2024 · e8cc709 · e8cc709
2 parents 8d87321 + af91f53
commit e8cc709
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java b/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java
@@ -38,6 +38,8 @@ public SwerveTelemetry(double maxSpeed) {
   private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
   private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
 
+  StructPublisher<Pose2d> posePub = inst.getStructTopic("SkibidiPose", Pose2d.struct).publish();
+
   /* Robot speeds for general checking */
   private final NetworkTable driveStats = inst.getTable("Drive");
   private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
@@ -93,6 +95,7 @@ public void telemeterize(SwerveDriveState state) {
     Pose2d pose = state.Pose;
     fieldTypePub.set("Field2d");
     fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()});
+    posePub.set(pose);
 
     /* Telemeterize the robot's general speeds */
     double currentTime = Utils.getCurrentTimeSeconds();