From 0cf866d19d5b870e5258a6ae7f6b8e20a48ecdc3 Mon Sep 17 00:00:00 2001
From: Audrey <audrey3zheng@gmail.com>
Date: Fri, 4 Oct 2024 16:11:02 -0700
Subject: [PATCH] fix gyro stabilization

---
 .../java/common/hardware/camera/Camera.java     | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/main/java/common/hardware/camera/Camera.java b/src/main/java/common/hardware/camera/Camera.java
index 32d4f70..c88bc11 100644
--- a/src/main/java/common/hardware/camera/Camera.java
+++ b/src/main/java/common/hardware/camera/Camera.java
@@ -16,8 +16,10 @@
 import common.utility.Log;
 import common.utility.shuffleboard.NAR_Shuffleboard;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.MathUtil;
 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;
 
@@ -52,6 +54,7 @@ public class Camera {
     public boolean isDisabled = false;
     public Transform3d offset;
     public double distance;
+    public Pose2d updatedPose;
 
     /**
      * Creates a Camera object
@@ -230,6 +233,18 @@ public boolean isValidTarget(PhotonTrackedTarget target) {
             !ignoredTags.contains(Integer.valueOf(target.getFiducialId()));
     }
 
+    /**
+     * Overrides the pose's rotation with the gyro angle
+     * @param result
+     * @return
+     */
+    public Pose2d getGyroPose(PhotonPipelineResult result) {
+        Pose2d pose = getPose(result).get();
+        Rotation2d gyroAngle = Rotation2d.fromDegrees(MathUtil.angleModulus(getGyroAngle()));
+        updatedPose = new Pose2d(pose.getX(), pose.getY(), gyroAngle); 
+        return updatedPose;
+    }
+
     public double getDistance(PhotonTrackedTarget target) {
         return target.getBestCameraToTarget().getTranslation().toTranslation2d().getNorm();
     }
@@ -276,7 +291,7 @@ public static void disableAll() {
     }
 
     public void initShuffleboard() {
-        NAR_Shuffleboard.addData(camera.getName(), "Estimated Pose", () -> estimatedPose.toString(), 0, 0, 4, 1);
+        NAR_Shuffleboard.addData(camera.getName(), "Estimated Pose", () -> updatedPose.toString(), 0, 0, 4, 1);
         NAR_Shuffleboard.addData(camera.getName(), "Distance", () -> distance, 0, 1, 2, 1);
         NAR_Shuffleboard.addData(camera.getName(), "Is Disabled", () -> isDisabled, 2, 1, 1, 1);
         NAR_Shuffleboard.addData(camera.getName(), "Has target", () -> result.hasTargets(), 3, 2, 1, 1);