From ec8f89149a76cab469a46c716c813c35d8a6e349 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 1 Sep 2025 15:20:06 -0700 Subject: [PATCH] Publish rotation from Pigeon to NetworkTables 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 --- src/main/java/com/team2813/subsystems/Drive.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 8a37a0c3..b46a8d60 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/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 anglePublisher; /** This measurement is IN INCHES */ private static final double WHEEL_RADIUS_IN = 1.537; @@ -330,6 +331,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()); @@ -534,9 +536,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); Pose2d drivePose = getPose(); currentPosePublisher.set(drivePose);