From ecbd88a19bbce5c98913f6f606016a254482e9be Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Mon, 6 Jan 2025 20:56:51 -0500 Subject: [PATCH 1/2] Added Some Vision Constants --- .../java/frc/robot/vision/VisionConfig.java | 167 ++++++++++++++++++ 1 file changed, 167 insertions(+) diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index ffa05d7..70efc26 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -1,5 +1,172 @@ package frc.robot.vision; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + public class VisionConfig { + public static final String centerPoseCam = "Center_Pose_Cam"; + public static final String leftPoseCam = "Left_Pose_Cam"; + public static final String rightPoseCam = "Right_Pose_Cam"; + public static final String driveCam = "Drive_Cam"; + + //TODO: when photonlib is added, create photon cameras with names + + + //TODO: config + public static final Transform3d robotToCenterCam = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d robotToLeftCam = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d robotToRightCam = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + + //APRIL TAG ID COORDS + //XYZ Origin (0, 0) is established in the bottom left corner of the field + + public static final Transform3d aprilTagID1 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID2 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID3 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID4 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID5 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID6 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID7 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID8 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID9 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID10 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID11 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID12 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID13 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID14 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID15 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID16 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID17 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID18 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID19 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID20 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID21 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); + + public static final Transform3d aprilTagID22 = + new Transform3d( + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + ); } From e3525945b0c41755ee5afb66260a29c75d97f8a5 Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Sat, 11 Jan 2025 14:45:25 -0500 Subject: [PATCH 2/2] Vision Constants Update --- .../java/frc/robot/vision/VisionConfig.java | 162 +++--------------- .../frc/robot/vision/VisionPoseEstimator.java | 2 + 2 files changed, 25 insertions(+), 139 deletions(-) diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index 70efc26..d7db065 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -1,5 +1,8 @@ package frc.robot.vision; +import org.photonvision.PhotonCamera; + +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -7,13 +10,22 @@ public class VisionConfig { - public static final String centerPoseCam = "Center_Pose_Cam"; - public static final String leftPoseCam = "Left_Pose_Cam"; - public static final String rightPoseCam = "Right_Pose_Cam"; - public static final String driveCam = "Drive_Cam"; + //Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard + public static final String centerPoseCamName = "Center_Pose_Cam"; + public static final String leftPoseCamName = "Left_Pose_Cam"; + public static final String rightPoseCamName = "Right_Pose_Cam"; + public static final String driveCamName = "Drive_Cam"; + + + //Creates all Photoncameras that will be used in pose estimator and commands + public static PhotonCamera centerCam = new PhotonCamera(centerPoseCamName); + public static PhotonCamera leftCam = new PhotonCamera(leftPoseCamName); + public static PhotonCamera rightCam = new PhotonCamera(rightPoseCamName); + public static PhotonCamera driveCam = new PhotonCamera(driveCamName); + + //Creates field layout for AprilTags + public static AprilTagFields aprilTagField = AprilTagFields.k2025Reefscape; - //TODO: when photonlib is added, create photon cameras with names - //TODO: config public static final Transform3d robotToCenterCam = @@ -28,145 +40,17 @@ public class VisionConfig { new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) ); - public static final Transform3d robotToRightCam = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - - //APRIL TAG ID COORDS - //XYZ Origin (0, 0) is established in the bottom left corner of the field - - public static final Transform3d aprilTagID1 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID2 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID3 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID4 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID5 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID6 = + public static final Transform3d robotToRightCam = new Transform3d( new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) ); - - public static final Transform3d aprilTagID7 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID8 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID9 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID10 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID11 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID12 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID13 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID14 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID15 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID16 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID17 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID18 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID19 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID20 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d aprilTagID21 = + + public static final Transform3d robotToDriveCam = new Transform3d( new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) + new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) ); - public static final Transform3d aprilTagID22 = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); + } diff --git a/src/main/java/frc/robot/vision/VisionPoseEstimator.java b/src/main/java/frc/robot/vision/VisionPoseEstimator.java index 9e7b201..fa10600 100644 --- a/src/main/java/frc/robot/vision/VisionPoseEstimator.java +++ b/src/main/java/frc/robot/vision/VisionPoseEstimator.java @@ -1,5 +1,7 @@ package frc.robot.vision; +import edu.wpi.first.apriltag.AprilTagFieldLayout; + public class VisionPoseEstimator { }