From 6577e94715af06a82cb4433122d1f5ca7c97c852 Mon Sep 17 00:00:00 2001
From: taj-maharj08 <arjunmahajan@billericak12.com>
Date: Tue, 4 Feb 2025 19:30:20 -0500
Subject: [PATCH] fix camera offsets

---
 src/main/java/frc/robot/RobotContainer.java          | 12 ++++++------
 .../frc/robot/subsystems/Vision/VisionConstants.java |  8 ++++----
 .../frc/robot/subsystems/drivetrain/DriveToPose.java |  9 ++++-----
 3 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 62864c7..ca01829 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -79,21 +79,21 @@ public RobotContainer(){
     
     new VisionIOPhotonVision("front-right-cam", new Transform3d(new Translation3d(
         Units.inchesToMeters(7.16),
-        Units.inchesToMeters(-10.92),
+        Units.inchesToMeters(10.92),
         Units.inchesToMeters(9.39)),
         new Rotation3d(
         Units.degreesToRadians(0.0),
         Units.degreesToRadians(-21.173),
-        Units.degreesToRadians(-20)))),
+        Units.degreesToRadians(-40)))),
 
     new VisionIOPhotonVision("front-left-cam", new Transform3d(new Translation3d(
-        Units.inchesToMeters(11.151),
-        Units.inchesToMeters(6.43),
-        Units.inchesToMeters(9.41)),
+        Units.inchesToMeters(7.211),
+        Units.inchesToMeters(-10.60),
+        Units.inchesToMeters(9.411)),
         new Rotation3d(
         Units.degreesToRadians(0.0),
         Units.degreesToRadians(-25.414),
-        Units.degreesToRadians(-40)))));
+        Units.degreesToRadians(-20)))));
 }
 
 
diff --git a/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java
index 78c366d..6e1fc40 100644
--- a/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java
+++ b/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java
@@ -24,13 +24,13 @@ public class VisionConstants {
       AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
   // Camera names, must match names configured on coprocessor
-  public static String camera0Name = "back-left-cam";
-  public static String camera1Name = "front-cam";
+  // public static String camera0Name = "back-left-cam";
+  // public static String camera1Name = "front-cam";
 
   // Robot to camera transforms
   // (Not used by Limelight, configure in web UI instead)
-  public static Transform3d robotToCamera0 =
-      new Transform3d(-0.3302, -0.3302, 0, new Rotation3d((3/4)*Math.PI, 0.2, 0.0));
+  // public static Transform3d robotToCamera0 =
+  //     new Transform3d(-0.3302, -0.3302, 0, new Rotation3d((3/4)*Math.PI, 0.2, 0.0));
 //   public static Transform3d robotToCamera1 =
 //       new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));
 
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DriveToPose.java b/src/main/java/frc/robot/subsystems/drivetrain/DriveToPose.java
index 57ea81d..5391400 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/DriveToPose.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/DriveToPose.java
@@ -20,7 +20,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 
-import static frc.robot.subsystems.vision.VisionConstants.robotToCamera0;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -29,15 +28,15 @@ public class DriveToPose extends Command {
   private final CommandSwerveDrivetrain m_drivetrain;
   private final ProfiledPIDController m_translationController, m_thetaController;
   private Translation2d m_lastSetpointTranslation;
-  private Pose2d m_pose;
   private Pose2d m_goalPose;
   private SwerveRequest.ApplyFieldSpeeds m_drive;
+  Transform2d m_shift;
 
 
   public DriveToPose(CommandSwerveDrivetrain drivetrain, Transform2d shift) {
-    m_pose = drivetrain.findClosestNode().transformBy(shift);
+    m_shift = shift;
     m_translationController =
-        new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2, 2));
+        new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2, 2));
     m_thetaController =
         new ProfiledPIDController(
             5.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1 * Math.PI, 1 * Math.PI));
@@ -52,7 +51,7 @@ public DriveToPose(CommandSwerveDrivetrain drivetrain, Transform2d shift) {
 
   @Override
   public void initialize() {
-    m_goalPose = m_pose;
+    m_goalPose = m_drivetrain.findClosestNode().transformBy(m_shift);
     Pose2d initialPose = m_drivetrain.getState().Pose;
 
     m_translationController.setTolerance(0.05);