Update camera pos constants

robototes · Feb 7, 2025 · 70c4ea4 · 70c4ea4
1 parent dba8ee1
commit 70c4ea4
Showing 1 changed file with 34 additions and 37 deletions.
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -40,34 +40,33 @@
  */
 public class VisionSubsystem extends SubsystemBase {
   // don't mind the value is never used part it is used
-  private static double CAMERA_X_POS_METERS;
-  private static double CAMERA_Y_POS_METERS = 0;
-  private static double CAMERA_Z_POS_METERS;
-  private static double CAMERA_ROLL = 0;
-  private static double CAMERA_PITCH;
-  private static double CAMERA_YAW = 0;
-  private static double CAMERA_YAW_BACK;
-
-  public static final Transform3d ROBOT_TO_CAM =
-      new Transform3d(
-          CAMERA_X_POS_METERS = Units.inchesToMeters(27.0 / 2.0 - 0.94996),
-          CAMERA_Y_POS_METERS,
-          CAMERA_Z_POS_METERS = Units.inchesToMeters(8.12331),
-          new Rotation3d(CAMERA_ROLL, CAMERA_PITCH = Units.degreesToRadians(-30), CAMERA_YAW));
-
-  public static final Transform3d ROBOT_TO_CAM_BACK =
-      new Transform3d(
-          CAMERA_X_POS_METERS = Units.inchesToMeters(27.0 / 2.0 - 0.94996) - 0.62,
-          CAMERA_Y_POS_METERS,
-          CAMERA_Z_POS_METERS = Units.inchesToMeters(8.12331),
-          new Rotation3d(
-              CAMERA_ROLL,
-              CAMERA_PITCH = Units.degreesToRadians(-30),
-              CAMERA_YAW_BACK = Units.degreesToRadians(180)));
+  private static final double CAMERA_X_POS_METERS_FRONT = Units.inchesToMeters(27.0 / 2.0 - 0.94996);
+  private static final double CAMERA_X_POS_METERS_BACK = Units.inchesToMeters(27.0 / 2.0 - 0.94996) - 0.62;
+  private static final double CAMERA_Y_POS_METERS = 0;
+  private static final double CAMERA_Z_POS_METERS = Units.inchesToMeters(8.12331);
+  private static final double CAMERA_ROLL = 0;
+  private static final double CAMERA_PITCH_FRONT = Units.degreesToRadians(90);
+  private static final double CAMERA_PITCH_BACK = Units.degreesToRadians(45);
+  private static final double CAMERA_YAW_FRONT = 0;
+  private static final double CAMERA_YAW_BACK = Units.degreesToRadians(180);
+
+  public static final Transform3d ROBOT_TO_CAM = new Transform3d(
+      CAMERA_X_POS_METERS_FRONT,
+      CAMERA_Y_POS_METERS,
+      CAMERA_Z_POS_METERS,
+      new Rotation3d(CAMERA_ROLL, CAMERA_PITCH_FRONT, CAMERA_YAW_FRONT));
+
+  public static final Transform3d ROBOT_TO_CAM_BACK = new Transform3d(
+      CAMERA_X_POS_METERS_BACK,
+      CAMERA_Y_POS_METERS,
+      CAMERA_Z_POS_METERS,
+      new Rotation3d(
+          CAMERA_ROLL,
+          CAMERA_PITCH_BACK,
+          CAMERA_YAW_BACK));
 
   // TODO Measure these
-  private static final Vector<N3> STANDARD_DEVS =
-      VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5));
+  private static final Vector<N3> STANDARD_DEVS = VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5));
 
   private final PhotonCamera photonCamera;
   private final PhotonCamera photonCamera2;
@@ -78,7 +77,7 @@ public class VisionSubsystem extends SubsystemBase {
   private final DrivebaseWrapper aprilTagsHelper;
 
   // These are always set with every pipeline result
-  private PhotonPipelineResult result = null;
+  private PhotonPipelineResult latestResult = null;
 
   private Optional<EstimatedRobotPose> latestPose = Optional.empty();
 
@@ -87,8 +86,7 @@ public class VisionSubsystem extends SubsystemBase {
   private double lastRawTimestampSeconds = 0;
   private Pose2d lastFieldPose = new Pose2d(-1, -1, new Rotation2d());
 
-  private static final AprilTagFieldLayout fieldLayout =
-      AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
+  private static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
 
   public VisionSubsystem(DrivebaseWrapper aprilTagsHelper) {
     robotField = new Field2d();
@@ -97,12 +95,10 @@ public VisionSubsystem(DrivebaseWrapper aprilTagsHelper) {
     rawVisionFieldObject = robotField.getObject("RawVision");
     photonCamera = new PhotonCamera(Hardware.FRONT_CAM);
     photonCamera2 = new PhotonCamera(Hardware.BACK_CAM);
-    photonPoseEstimatorFrontCamera =
-        new PhotonPoseEstimator(
-            fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ROBOT_TO_CAM);
-    photonPoseEstimatorBackCamera =
-        new PhotonPoseEstimator(
-            fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ROBOT_TO_CAM_BACK);
+    photonPoseEstimatorFrontCamera = new PhotonPoseEstimator(
+        fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ROBOT_TO_CAM);
+    photonPoseEstimatorBackCamera = new PhotonPoseEstimator(
+        fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ROBOT_TO_CAM_BACK);
 
     var networkTables = NetworkTableInstance.getDefault();
     networkTables.addListener(
@@ -163,11 +159,12 @@ public boolean hasTargets() {
   }
 
   public int getNumTargets() {
-    return result == null ? -1 : result.getTargets().size();
+    return latestResult == null ? -1 : latestResult.getTargets().size();
   }
 
   /**
-   * Calculates the robot pose using the best target. Returns null if there is no known robot pose.
+   * Calculates the robot pose using the best target. Returns null if there is no
+   * known robot pose.
    *
    * @return The calculated robot pose in meters.
    */