Added vision objects and robotToCam(both cameras)

chopshop-166 · Jan 18, 2025 · 8b8c7b0 · 8b8c7b0
1 parent c1f5dfb
commit 8b8c7b0
Showing 1 changed file with 18 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -62,21 +62,28 @@ public class Drive extends LoggedSubsystem<SwerveDriveData, SwerveDriveMap> {
 
     SwerveDrivePoseEstimator estimator;
 
-    // Cam mounted facing forward, half a meter forward of center, half a meter up
-    // from center.
-    public static final Transform3d kRobotToCam = new Transform3d(
-            new Translation3d(Units.inchesToMeters(-6.9965), Units.inchesToMeters(-3.029),
-                    Units.inchesToMeters(12.445)),
-            new Rotation3d(0, Units.degreesToRadians(-16.875), Units.degreesToRadians(-6.5 + 180)));
+    // Vision objects
+    private final PhotonCamera ShrimpCamFL;
+    private final PhotonCamera ShrimpCamFR;
+    private final PhotonPoseEstimator photonEstimatorFL;
+    private final PhotonPoseEstimator photonEstimatorFR;
+    private double lastEstTimestamp = 0;
 
     // The layout of the AprilTags on the field
     public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
-    // The standard deviations of our vision estimated poses, which affect
-    // correction rate
-    // (Fake values. Experiment and determine estimation noise on an actual robot.)
-    public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
-    public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
+    // Cam mounted 9.029 sideways of center, 9.029 in. forward of center, 9.75 in.
+    // up from center.
+    public static final Transform3d robotToCamFL = new Transform3d(
+            new Translation3d(Units.inchesToMeters(-9.029), Units.inchesToMeters(9.029),
+                    Units.inchesToMeters(9.75)),
+            new Rotation3d(0, Units.degreesToRadians(-45), Units.degreesToRadians(65.752)));
+
+    // Cam mounted facing __, __ forward of center, __ up from center.
+    public static final Transform3d RobotToCamFR = new Transform3d(
+            new Translation3d(Units.inchesToMeters(9.029), Units.inchesToMeters(9.029),
+                    Units.inchesToMeters(9.75)),
+            new Rotation3d(0, Units.degreesToRadians(45), Units.degreesToRadians(65.752)));
 
     public Drive(SwerveDriveMap map, DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation) {