From 721f3baf9d898dbb89bff4e8a71c0ee28c4fbd52 Mon Sep 17 00:00:00 2001
From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com>
Date: Wed, 5 Mar 2025 23:46:56 -0500
Subject: [PATCH 1/2] vision experiment

this is dumb
---
 .../java/frc/robot/vision/VisionConsumer.java | 15 +++++++
 .../robot/vision/commands/VisionCommand.java  | 43 +++++++++++++++++++
 2 files changed, 58 insertions(+)
 create mode 100644 src/main/java/frc/robot/vision/VisionConsumer.java
 create mode 100644 src/main/java/frc/robot/vision/commands/VisionCommand.java

diff --git a/src/main/java/frc/robot/vision/VisionConsumer.java b/src/main/java/frc/robot/vision/VisionConsumer.java
new file mode 100644
index 0000000..6bd0019
--- /dev/null
+++ b/src/main/java/frc/robot/vision/VisionConsumer.java
@@ -0,0 +1,15 @@
+// package frc.robot.vision;
+
+// import edu.wpi.first.math.Matrix;
+// import edu.wpi.first.math.geometry.Pose2d;
+// import edu.wpi.first.math.numbers.N1;
+// import edu.wpi.first.math.numbers.N3;
+
+// @FunctionalInterface
+// public interface VisionConsumer {
+//     public void addVisionMeasurement(
+//         Pose2d visionRobotPoseM,
+//         double timestampSeconds,
+//         Matrix<N3,N1> visionMeasurementStdDevs
+//     );
+// }
diff --git a/src/main/java/frc/robot/vision/commands/VisionCommand.java b/src/main/java/frc/robot/vision/commands/VisionCommand.java
new file mode 100644
index 0000000..1306e8c
--- /dev/null
+++ b/src/main/java/frc/robot/vision/commands/VisionCommand.java
@@ -0,0 +1,43 @@
+// package frc.robot.vision.commands;
+
+// import static frc.robot.vision.VisionConfig.camNames;
+// import static frc.robot.vision.VisionConfig.robotToCamTransforms;
+
+// import java.nio.channels.NetworkChannel;
+
+// import edu.wpi.first.math.geometry.Pose2d;
+// import edu.wpi.first.networktables.NetworkTableInstance;
+// import edu.wpi.first.networktables.StructPublisher;
+// import edu.wpi.first.wpilibj2.command.Command;
+// import frc.robot.vision.Vision;
+// import frc.robot.vision.VisionConsumer;
+
+// public class VisionCommand extends Command {
+//     private final Vision[] visions;
+//     private final VisionConsumer visionConsumer;
+
+//     public VisionCommand(VisionConsumer consumer){
+//         visions = new Vision[camNames.length];
+//         for (int i = 0; i < camNames.length; i++){
+//             visions[i] = new Vision(camNames[i], robotToCamTransforms[i]);
+//         }
+//         this.visionConsumer = consumer; 
+//     }
+
+//     @Override
+//     public void execute(){
+//         for (int i = 0; i < visions.length; i++){
+//             var vision = visions[i];
+//             var visionEst = vision.getEstimatedGlobalPose();
+//             visionEst.ifPresent(est -> {
+//                 var estStdDevs = vision.getEstimationStdDevs();
+//                 visionConsumer.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
+//             });
+//         }
+//     }
+
+//     @Override
+//     public boolean runsWhenDisabled(){
+//         return true;
+//     }
+// }

From 765a54182c14a46c26f07a0e2872b10014895a06 Mon Sep 17 00:00:00 2001
From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com>
Date: Thu, 6 Mar 2025 20:50:35 -0500
Subject: [PATCH 2/2] Vision V2.5

---
 src/main/java/frc/robot/vision/VisionConfig.java | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java
index de5f8ba..f5ba356 100644
--- a/src/main/java/frc/robot/vision/VisionConfig.java
+++ b/src/main/java/frc/robot/vision/VisionConfig.java
@@ -12,20 +12,25 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Unit;
 
 public class VisionConfig {
     
     // Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard
     public static final int TOTAL_CAMERAS = 4;
-    public static final String[] camNames = new String[] {"Center_Cam", "Left_Cam", "Right_Cam", "Drive_Cam"};
+    public static final String[] camNames = new String[] {"Left_Cam", "Right_Cam"}; //TODO: add center cam and drive cam
 
     //Camera Positions
     // TODO: config camera transforms
     public static final Transform3d[] robotToCamTransforms = new Transform3d[] {
-        new Transform3d(new Translation3d(), new Rotation3d()),
-        new Transform3d(new Translation3d(), new Rotation3d()),
-        new Transform3d(new Translation3d(), new Rotation3d()),
-        new Transform3d(new Translation3d(), new Rotation3d())
+        //left cam
+        new Transform3d(
+            new Translation3d(Units.inchesToMeters(-12.436), Units.inchesToMeters(11.677), Units.inchesToMeters(7.413)),
+            new Rotation3d(0,Units.degreesToRadians(15),Units.degreesToRadians(-20))),
+        //right cam
+        new Transform3d(
+            new Translation3d(Units.inchesToMeters(-12.436), Units.inchesToMeters(-11.677), Units.inchesToMeters(7.413)), 
+            new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(20)))
     }; 
 
     // Creates field layout for AprilTags