Publish rotation from Pigeon to NetworkTables (#3)

We were seeing weird positions from PhotonVision when testing on the practice
field. The team was undsure what angle the pigeon was reporting, so we decided
that we should publish that data to Network Tables.

Co-authored-by: Arthur Hung

Author: kcooney

src/main/java/com/team2813/subsystems/Drive.java

@@ -106,6 +106,7 @@ public class Drive extends SubsystemBase implements AutoCloseable {
   private final DoubleArrayPublisher modulePositionsPublisher;
   private final DoublePublisher ambiguityPublisher =
       NetworkTableInstance.getDefault().getDoubleTopic("Ambiguity").publish();
+  private final StructPublisher<Rotation3d> anglePublisher;
 
   /** This measurement is <em>IN INCHES</em> */
   private static final double WHEEL_RADIUS_IN = 1.537;
@@ -336,6 +337,7 @@ public Drive(
         networkTable.getStructArrayTopic("actual state", SwerveModuleState.struct).publish();
     currentPosePublisher = networkTable.getStructTopic("current pose", Pose2d.struct).publish();
     modulePositionsPublisher = networkTable.getDoubleArrayTopic("module positions").publish();
+    anglePublisher = networkTable.getStructTopic("pigeon angle", Rotation3d.struct).publish();
 
     setDefaultCommand(createDefaultCommand());
 
@@ -540,9 +542,11 @@ public void periodic() {
     // Publish data to NetworkTables
     expectedStatePublisher.set(drivetrain.getState().ModuleTargets);
     actualStatePublisher.set(drivetrain.getState().ModuleStates);
+    Rotation3d rotation = getRotation3d();
     if (config.usePnpDistanceTrigSolveStrategy) {
-      photonPoseEstimator.addHeadingData(Timer.getTimestamp(), getRotation3d());
+      photonPoseEstimator.addHeadingData(Timer.getTimestamp(), rotation);
     }
+    anglePublisher.set(rotation);
     photonPoseEstimator.update(this::handlePhotonPose);
     // Thoughts about moving away from functional programing paradigm here to just simpler,
     // procedural programming, e.g.,