From 3a05efc5f15d2bd7e4b529e7a18a577c05ab538f Mon Sep 17 00:00:00 2001
From: WindingMotor <kid.subudhi@gmail.com>
Date: Tue, 31 Dec 2024 14:53:23 -0500
Subject: [PATCH] Camera transform logging

---
 .../frc/robot/constants/BuildConstants.java   | 10 +++++-----
 .../frc/robot/constants/CameraConstants.java  |  9 ++++++++-
 .../java/frc/robot/swerve/SUB_Swerve.java     | 20 +++++++++++++++++++
 3 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/constants/BuildConstants.java b/src/main/java/frc/robot/constants/BuildConstants.java
index 60ab4f7..bab2871 100644
--- a/src/main/java/frc/robot/constants/BuildConstants.java
+++ b/src/main/java/frc/robot/constants/BuildConstants.java
@@ -12,12 +12,12 @@ public final class BuildConstants {
 	public static final String MAVEN_GROUP = "";
 	public static final String MAVEN_NAME = "SwerveDrive2025";
 	public static final String VERSION = "unspecified";
-	public static final int GIT_REVISION = 16;
-	public static final String GIT_SHA = "aca65fc8cd819b1f7aba6ed9ab14a1a1eb282781";
-	public static final String GIT_DATE = "2024-12-30 13:45:34 EST";
+	public static final int GIT_REVISION = 20;
+	public static final String GIT_SHA = "6dbe868af98e109a67e392e499c0bcbbf013a67d";
+	public static final String GIT_DATE = "2024-12-31 13:39:27 EST";
 	public static final String GIT_BRANCH = "main";
-	public static final String BUILD_DATE = "2024-12-30 14:00:18 EST";
-	public static final long BUILD_UNIX_TIME = 1735585218107L;
+	public static final String BUILD_DATE = "2024-12-31 14:50:57 EST";
+	public static final long BUILD_UNIX_TIME = 1735674657151L;
 	public static final int DIRTY = 1;
 
 	private BuildConstants() {}
diff --git a/src/main/java/frc/robot/constants/CameraConstants.java b/src/main/java/frc/robot/constants/CameraConstants.java
index a08bb02..2a47d6b 100644
--- a/src/main/java/frc/robot/constants/CameraConstants.java
+++ b/src/main/java/frc/robot/constants/CameraConstants.java
@@ -9,6 +9,7 @@
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N1;
@@ -37,7 +38,7 @@ public enum Camera {
 				VecBuilder.fill(0.5, 0.5, 1)),
 		CENTER_CAM(
 				"center",
-				new Rotation3d(0, Units.degreesToRadians(18), 0),
+				new Rotation3d(0, Units.degreesToRadians(-145), 0),
 				new Translation3d(
 						Units.inchesToMeters(-4.628),
 						Units.inchesToMeters(-10.687),
@@ -64,4 +65,10 @@ public enum Camera {
 			this.multiTagStdDevs = multiTagStdDevs;
 		}
 	}
+
+	public static final Pose3d[] CAMERA_POSITIONS = {
+		new Pose3d(Camera.LEFT_CAM.translation, Camera.LEFT_CAM.rotation),
+		new Pose3d(Camera.RIGHT_CAM.translation, Camera.RIGHT_CAM.rotation),
+		new Pose3d(Camera.CENTER_CAM.translation, Camera.CENTER_CAM.rotation)
+	};
 }
diff --git a/src/main/java/frc/robot/swerve/SUB_Swerve.java b/src/main/java/frc/robot/swerve/SUB_Swerve.java
index dc4d3f1..87dfbec 100644
--- a/src/main/java/frc/robot/swerve/SUB_Swerve.java
+++ b/src/main/java/frc/robot/swerve/SUB_Swerve.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -25,6 +26,7 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.CameraConstants;
 import frc.robot.constants.RobotConstants;
 import frc.robot.vision.SUB_Vision;
 import org.littletonrobotics.junction.Logger;
@@ -68,6 +70,24 @@ public void periodic() {
 					"Vision/CurrentEstimatedPose", vision.getEstimatedGlobalPose().get().estimatedPose);
 		}
 
+		// Convert Pose2d to Pose3d for camera position transformation
+		Pose3d robotPose3d = new Pose3d(inputs.robotPose);
+
+		// Transform camera positions to global coordinate system
+		Pose3d[] globalCameraPositions = new Pose3d[CameraConstants.CAMERA_POSITIONS.length];
+		for (int i = 0; i < CameraConstants.CAMERA_POSITIONS.length; i++) {
+			globalCameraPositions[i] =
+					robotPose3d.transformBy(
+							new Transform3d(
+									CameraConstants.CAMERA_POSITIONS[i].getTranslation(),
+									CameraConstants.CAMERA_POSITIONS[i].getRotation()));
+		}
+
+		// Record camera positions in global coordinate system
+		// In AScope set camera positions to cone object, and the pointy end is where the camera is looking at
+		// If you want to calibrate the postion set camera positions to transform object instead
+		Logger.recordOutput("CameraPositions", globalCameraPositions);
+
 		// Update inputs
 		io.updateInputs(inputs);