Log the vision pose

Iron-Panthers · Jan 5, 2025 · f7929bb · f7929bb
1 parent c05c414
commit f7929bb
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -90,8 +90,9 @@ public void addVisionMeasurement(EstimatedRobotPose pose) {
         || poseBuffer.getInternalBuffer().lastKey() < poseBufferSizeSeconds) {
       return;
     }
-    poseBuffer.addSample(pose.timestampSeconds, pose.estimatedPose.toPose2d());
+    // poseBuffer.addSample(pose.timestampSeconds, pose.estimatedPose.toPose2d()); 
     poseEstimator.addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds);
+
   }
 
   public void resetPose(Pose2d pose) {
@@ -109,4 +110,5 @@ public Pose2d getOdometryPose() {
   public Pose2d getEstimatedPose() {
     return estimatedPose;
   }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionPoseEstimation.java b/src/main/java/frc/robot/subsystems/vision/VisionPoseEstimation.java
@@ -4,6 +4,7 @@
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -12,6 +13,7 @@
 import java.util.ArrayList;
 import java.util.Optional;
 
+import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
@@ -22,6 +24,8 @@
 public class VisionPoseEstimation extends SubsystemBase {
   private ArrayList<PhotonCamera> cameras;
 
+  private Pose2d visionEstimatedPose = new Pose2d();
+
 
   private static final Transform3d CAMERA1_TO_CENTER =
       new Transform3d( // FIXME
@@ -56,14 +60,36 @@ public void periodic() {
   }
 
   public void visionUpdatePose() {
+    double xSum = 0;
+    double ySum = 0;
+    double angleSum = 0;
+    int targets = 0;
     for (int i = 0; i < cameras.size(); i++) {
       final Optional<EstimatedRobotPose> optionalEstimatedPose;
       if (cameras.get(i).getAllUnreadResults().size() > 0) {
         PhotonPipelineResult result = cameras.get(i).getAllUnreadResults().get(0);
         optionalEstimatedPose = poseEstimators.get(i).update(result);
         EstimatedRobotPose estimatedPose = optionalEstimatedPose.get();
         RobotState.getInstance().addVisionMeasurement(estimatedPose);
+
+        estimatedPose.estimatedPose.toPose2d();
+        xSum += estimatedPose.estimatedPose.toPose2d().getX();
+        ySum += estimatedPose.estimatedPose.toPose2d().getY();
+        angleSum += estimatedPose.estimatedPose.toPose2d().getRotation().getRadians();
+        targets++;
       }
     }
+    if (targets > 0) {
+      xSum /= targets;
+      ySum /= targets;
+      angleSum /= targets;
+      visionEstimatedPose = new Pose2d(xSum, ySum, new Rotation2d(angleSum));
+    }
+  }
+
+
+  @AutoLogOutput(key = "RobotState/VisionPose")
+  public Pose2d getVisionPose() {
+    return visionEstimatedPose;
   }
 }