Vision Lineup fixes (#114)

* Changes * change offset to subtraction --------- Co-authored-by: minhnguyenbhs <[email protected]>
team401 · Feb 3, 2025 · 2111373 · 2111373
1 parent 84e8cbf
commit 2111373
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 6 deletions.
diff --git a/gradle.properties b/gradle.properties
@@ -1,2 +1,2 @@
-version=2025.1.6
+version=2025.1.7
 
diff --git a/vision/src/main/java/coppercore/vision/VisionIO.java b/vision/src/main/java/coppercore/vision/VisionIO.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
 import org.littletonrobotics.junction.AutoLog;
 
 /**
@@ -34,7 +35,12 @@ public static record PoseObservation(
             double averageTagDistance) {}
 
     public static record SingleTagObservation(
-            int tagId, double timestamp, double distance3D, Rotation2d tx, Rotation2d ty) {}
+            int tagId,
+            double timestamp,
+            double distance3D,
+            Transform3d cameraToTarget,
+            Rotation2d tx,
+            Rotation2d ty) {}
     ;
 
     public default void updateInputs(VisionIOInputs inputs) {}

diff --git a/vision/src/main/java/coppercore/vision/VisionIOPhotonReal.java b/vision/src/main/java/coppercore/vision/VisionIOPhotonReal.java
@@ -74,6 +74,7 @@ public void updateInputs(VisionIOInputs inputs) {
                                     target.getFiducialId(),
                                     result.getTimestampSeconds(),
                                     target.getBestCameraToTarget().getTranslation().getNorm(),
+                                    target.getBestCameraToTarget(),
                                     new Rotation2d(target.getYaw()),
                                     new Rotation2d(target.getPitch())));
                 }
@@ -133,8 +134,9 @@ public void updateInputs(VisionIOInputs inputs) {
                                     target.fiducialId,
                                     result.getTimestampSeconds(),
                                     target.getBestCameraToTarget().getTranslation().getNorm(),
-                                    new Rotation2d(target.getYaw()),
-                                    new Rotation2d(target.getPitch())));
+                                    target.getBestCameraToTarget(),
+                                    new Rotation2d(Math.toRadians(target.getYaw())),
+                                    new Rotation2d(Math.toRadians(target.getPitch()))));
                 }
             }
         }

diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java
@@ -130,10 +130,10 @@ public DistanceToTag getDistanceErrorToTag(
         // calculate strafe and forward distances required to get to tag
         double crossTrackDistance =
                 distanceXYPlane * Math.sin(tagObserved.tx().minus(new Rotation2d()).getRadians())
-                        + crossTrackOffsetMeters;
+                        - crossTrackOffsetMeters;
         double alongTrackDistance =
                 distanceXYPlane * Math.cos(tagObserved.tx().minus(new Rotation2d()).getRadians())
-                        + alongTrackOffsetMeters;
+                        - alongTrackOffsetMeters;
 
         return new DistanceToTag(crossTrackDistance, alongTrackDistance, true);
     }