real camera poses

grt192 · Mar 29, 2024 · 1759aca · 1759aca
1 parent 3ae8e5a
commit 1759aca
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -68,7 +68,7 @@ public static class SwerveConstants {
 
         public static final int BL_DRIVE = 4;
         public static final int BL_STEER = 5;
-        public static final double BL_OFFSET = 5.79 + Math.PI * 7.0 / 4 + Math.PI * 1.0/8 + Math.PI * 3.0/4;
+        public static final double BL_OFFSET = 5.79 + Math.PI * 7.0 / 4 + Math.PI * 1.0 / 8 + Math.PI * 3.0 / 4;
 
         public static final int BR_DRIVE = 6;
         public static final int BR_STEER = 7;
@@ -216,25 +216,39 @@ public static class LEDConstants {
     /** Constants for vision. */
     public static final class VisionConstants {
         public static final String VISION_TABLE_KEY = "Vision";
+        public static final Boolean USE_REAL_CAMERA_POSES = true; 
 
-        //TODO: Calculate real (not from CAD) poses.
         public static final PhotonCamera FRONT_RIGHT_CAMERA = new PhotonCamera("front right");
         public static final Transform3d FRONT_RIGHT_CAMERA_POSE = new Transform3d(
             new Translation3d(+0.238, -0.357, +0.662), /* in meters */
             new Rotation3d(new Quaternion(+0.01331, +0.98037, -0.09192, +0.17394)) /* W X Y Z */
         );
 
         public static final PhotonCamera BACK_LEFT_CAMERA = new PhotonCamera("back left");
-        public static final Transform3d  BACK_LEFT_CAMERA_POSE = new Transform3d(
-            new Translation3d(-0.126, +0.298, +0.563), /* in meters */
-            new Rotation3d(new Quaternion(-0.13815, +0.17069, +0.97523, +0.02661)) /* W X Y Z */
-        );
+        public static final Transform3d  BACK_LEFT_CAMERA_POSE = USE_REAL_CAMERA_POSES
+            /* Calculated Real Pose */
+            ? new Transform3d(
+                new Translation3d(-0.120, +0.305, +0.572), /* in meters */
+                new Rotation3d(new Quaternion(-0.11148, +0.15802, +0.98081, +0.02497)) /* W X Y Z */
+            )
+            /* CAD Pose */
+            : new Transform3d(
+                new Translation3d(-0.126, +0.298, +0.563), /* in meters */
+                new Rotation3d(new Quaternion(-0.13815, +0.17069, +0.97523, +0.02661)) /* W X Y Z */
+            );
 
         public static final PhotonCamera BACK_RIGHT_CAMERA = new PhotonCamera("back right");
-        public static final Transform3d  BACK_RIGHT_CAMERA_POSE = new Transform3d(
-            new Translation3d(-0.130, -0.294, +0.567), /* in meters */
-            new Rotation3d(new Quaternion(-0.09190, +0.17498, +0.01993, +0.98007)) /* W X Y Z */
-        );
+        public static final Transform3d  BACK_RIGHT_CAMERA_POSE = USE_REAL_CAMERA_POSES
+            /* Calculated Real Pose */
+            ? new Transform3d(
+                new Translation3d(-0.161, -0.334, +0.516), /* in meters */
+                new Rotation3d(new Quaternion(-0.09005, +0.18402, +0.02004, +0.97858)) /* W X Y Z */
+            )
+            /* CAD Pose */
+            : new Transform3d(
+                new Translation3d(-0.130, -0.294, +0.567), /* in meters */
+                new Rotation3d(new Quaternion(-0.09190, +0.17498, +0.01993, +0.98007)) /* W X Y Z */
+            );
 
         public static final PhotonCamera NOTE_CAMERA = new PhotonCamera("cap");
     }

diff --git a/src/main/java/frc/robot/commands/vision/CalculateBackCameraTransformCommand.java b/src/main/java/frc/robot/commands/vision/CalculateBackCameraTransformCommand.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj2.command.Command;
 import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 /**
@@ -21,12 +22,17 @@ public class CalculateBackCameraTransformCommand extends Command {
     public CalculateBackCameraTransformCommand(PhotonCamera backLeft, PhotonCamera backRight) {
         this.backLeft = backLeft;
         this.backRight = backRight;
+        System.out.println("Starting . . .");
     }
 
     @Override
     public void execute() {
-        backLeftTarget = backLeft.getLatestResult().getBestTarget();
-        backRightTarget = backRight.getLatestResult().getBestTarget();
+        PhotonPipelineResult backLeftResult = backLeft.getLatestResult();
+        PhotonPipelineResult backRightResult = backRight.getLatestResult();
+        backLeftTarget = backLeftResult.getBestTarget();
+        backRightTarget = backRightResult.getBestTarget();
+
+        System.out.println((backLeftTarget != null) + "   " + (backRightTarget != null));
 
         if (backLeftTarget == null || backRightTarget == null) {
             backLeftTarget = null;
@@ -39,6 +45,10 @@ public void execute() {
             backRightTarget = null;
             return;
         }
+
+        if (Math.abs(backRightResult.getTimestampSeconds() - backLeftResult.getTimestampSeconds()) > 0.05) {
+            return;
+        }
 
         backLeftToBackRight = backLeftTarget.getBestCameraToTarget()
                         .plus(backRightTarget.getBestCameraToTarget().inverse());