fix: use rotation3ds for returning gamepiece positions

flamingchickens1540 · Sep 16, 2024 · eb4eb21 · eb4eb21
1 parent c92b957
commit eb4eb21
Show file tree

Hide file tree

Showing 5 changed files with 15 additions and 11 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -110,10 +110,8 @@ public static class Vision {
         public static class AprilTag {
             public static final String FRONT_CAMERA_NAME = "limelight-front";
             public static final String REAR_CAMERA_NAME = "limelight-rear";
-            public static final String VISION_CAMERA_NAME = "limelight-vision";
             public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0));
             public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(Math.PI, Math.toRadians(10), Math.PI+Math.toRadians(1.55)));
-            public static final Pose3d VISION_CAMERA_POSE = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); //TODO FIND THIS POSE
             public static final boolean TAKE_SNAPSHOTS = true;
             public static final double SNAPSHOT_PERIOD_SECS = 1;
             public static final double XY_STD_DEV_COEFF = 0.1;
@@ -134,6 +132,7 @@ public static class AprilTag {
         public static class Gamepiece {
             public static final String CAMERA_NAME = "limelight-notevision";
             public static final int PIPELINE_INDEX = 0;
+            public static final Pose3d CAMERA_POSE = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); //TODO FIND THIS POSE
         }
     }
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java
@@ -25,8 +25,7 @@ public void periodic() {
     public GamepieceDetection getLatestDetection() {
         return new GamepieceDetection(
                 inputs.lastDetectionTimestampSecs,
-                inputs.targetPitchRads,
-                inputs.targetYawRads,
+                inputs.targetRotation.rotateBy(CAMERA_POSE.getRotation().unaryMinus()),
                 inputs.targetArea,
                 inputs.targetClass);
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIO.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIO.java
@@ -1,14 +1,14 @@
 package org.team1540.robot2024.subsystems.vision.gamepiece;
 
+import edu.wpi.first.math.geometry.Rotation3d;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface NoteVisionIO {
     @AutoLog
     class NoteVisionIOInputs {
         public boolean hasDetection = false;
         public double lastDetectionTimestampSecs = 0.0;
-        public double targetPitchRads = 0.0;
-        public double targetYawRads = 0.0;
+        public Rotation3d targetRotation = new Rotation3d(); // Camera-relative target rotation
         public double targetArea = 0.0;
         public String targetClass = "";
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java
@@ -1,5 +1,6 @@
 package org.team1540.robot2024.subsystems.vision.gamepiece;
 
+import edu.wpi.first.math.geometry.Rotation3d;
 import org.team1540.robot2024.util.vision.LimelightHelpers;
 
 import static org.team1540.robot2024.Constants.Vision.Gamepiece.*;
@@ -22,8 +23,12 @@ public void updateInputs(NoteVisionIOInputs inputs) {
                     (LimelightHelpers.getLimelightNTTableEntry(name, "tv").getLastChange() / 1000000.0)
                             - ((LimelightHelpers.getLatency_Capture(name) + LimelightHelpers.getLatency_Pipeline(name))
                             / 1000.0);
-            inputs.targetPitchRads = Math.toRadians(LimelightHelpers.getTY(name));
-            inputs.targetYawRads = Math.toRadians(LimelightHelpers.getTX(name));
+            //TODO check rotation signs
+            inputs.targetRotation =
+                    new Rotation3d(
+                            0,
+                            -Math.toRadians(LimelightHelpers.getTY(name)),
+                            Math.toRadians(LimelightHelpers.getTX(name)));
             inputs.targetArea = LimelightHelpers.getTA(name);
             inputs.targetClass = LimelightHelpers.getNeuralClassID(name);
         }

diff --git a/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java b/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java
@@ -1,8 +1,9 @@
 package org.team1540.robot2024.util.vision;
 
+import edu.wpi.first.math.geometry.Rotation3d;
+
 public record GamepieceDetection(
         double timestampSecs,
-        double yawRads,
-        double pitchRads,
-        double areaRads,
+        Rotation3d rotation, // Robot relative gamepiece rotation
+        double area,
         String targetClass) {}