From d4aec9b2696a38b555d8f4a532773fe0dcaaf251 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Mon, 19 Jan 2026 18:04:08 +0200 Subject: [PATCH 1/9] Implemented DynamicCameraTransform, need to change and add Jdocs --- .../apriltagcamera/AprilTagCamera.java | 46 +++++++++++------ .../AprilTagCameraConstants.java | 7 ++- .../apriltagcamera/AprilTagCameraIO.java | 7 ++- .../DynamicCameraTransform.java | 50 +++++++++++++++++++ .../io/AprilTagLimelightIO.java | 6 +-- .../io/AprilTagPhotonCameraIO.java | 17 ++++--- .../io/AprilTagSimulationCameraIO.java | 19 +++++-- 7 files changed, 113 insertions(+), 39 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index c7689a5c..6f7797cc 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -1,6 +1,9 @@ package frc.trigon.robot.poseestimation.apriltagcamera; -import edu.wpi.first.math.geometry.*; +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.wpilibj.DriverStation; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; @@ -15,7 +18,7 @@ public class AprilTagCamera { protected final String name; private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); - private final Transform2d cameraToRobotCenter; + private final DynamicCameraTransform dynamicCameraTransform; private final StandardDeviations standardDeviations; private final AprilTagCameraIO aprilTagCameraIO; private Pose2d estimatedRobotPose = new Pose2d(); @@ -35,11 +38,29 @@ public class AprilTagCamera { public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotCenterToCamera, StandardDeviations standardDeviations) { + this(aprilTagCameraType, name, new DynamicCameraTransform(robotCenterToCamera), standardDeviations); + } + + /** + * Constructs a new AprilTagCamera. + * + * @param aprilTagCameraType the type of camera + * @param name the camera's name + * @param dynamicCameraTransform the transform of the robot's origin point to the camera. + * only the x, y and yaw values will be used for transforming the camera pose to the robot's center, + * to avoid more inaccuracies like pitch and roll. + * The reset will be used for creating a camera in simulation + * @param standardDeviations the initial calibrated standard deviations for the camera's estimated pose, + * will be changed as the distance from the tag(s) changes and the number of tags changes + */ + public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, + String name, DynamicCameraTransform dynamicCameraTransform, + StandardDeviations standardDeviations) { this.name = name; this.standardDeviations = standardDeviations; - this.cameraToRobotCenter = toTransform2d(robotCenterToCamera).inverse(); + this.dynamicCameraTransform = dynamicCameraTransform; - aprilTagCameraIO = AprilTagCameraIO.generateIO(aprilTagCameraType, name, robotCenterToCamera); + aprilTagCameraIO = AprilTagCameraIO.generateIO(aprilTagCameraType, name, dynamicCameraTransform); } public void update() { @@ -106,18 +127,18 @@ private Pose2d chooseBestRobotPose() { if (!inputs.hasConstrainedResult || isWithinBestTagRangeForAccurateSolvePNPResult()) return chooseBestNormalSolvePNPPose(); - return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose.toPose2d()); + return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose.toPose2d(), inputs.latestResultTimestampSeconds); } private Pose2d chooseBestNormalSolvePNPPose() { - final Pose2d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose.toPose2d()); + final Pose2d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose.toPose2d(), inputs.latestResultTimestampSeconds); if (inputs.bestCameraSolvePNPPose.equals(inputs.alternateCameraSolvePNPPose)) return bestPose; if (inputs.alternateCameraSolvePNPPose.getTranslation().toTranslation2d().getDistance(FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).getTranslation().toTranslation2d()) < 0.1 || DriverStation.isDisabled()) return bestPose; - final Pose2d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose.toPose2d()); + final Pose2d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose.toPose2d(), inputs.latestResultTimestampSeconds); final Rotation2d robotAngleAtResultTime = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(inputs.latestResultTimestampSeconds).getRotation(); final double bestAngleDifference = Math.abs(bestPose.getRotation().minus(robotAngleAtResultTime).getRadians()); @@ -126,8 +147,8 @@ private Pose2d chooseBestNormalSolvePNPPose() { return bestAngleDifference > alternateAngleDifference ? alternatePose : bestPose; } - private Pose2d cameraPoseToRobotPose(Pose2d cameraPose) { - return cameraPose.transformBy(cameraToRobotCenter); + private Pose2d cameraPoseToRobotPose(Pose2d cameraPose, double resultTimestampSeconds) { + return dynamicCameraTransform.calculate2dRobotPose(cameraPose, resultTimestampSeconds); } /** @@ -167,13 +188,6 @@ private void logUsedTags() { Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses); } - private Transform2d toTransform2d(Transform3d transform3d) { - final Translation2d robotCenterToCameraTranslation = transform3d.getTranslation().toTranslation2d(); - final Rotation2d robotCenterToCameraRotation = transform3d.getRotation().toRotation2d(); - - return new Transform2d(robotCenterToCameraTranslation, robotCenterToCameraRotation); - } - private double calculateAverageDistanceFromTags() { double totalDistance = 0; for (int visibleTagID : inputs.visibleTagIDs) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index bd693b84..f382e1ba 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -2,7 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.lib.hardware.RobotHardwareStats; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; @@ -10,7 +10,6 @@ import org.photonvision.PhotonPoseEstimator; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; -import frc.trigon.lib.hardware.RobotHardwareStats; import java.util.function.BiFunction; @@ -57,9 +56,9 @@ public enum AprilTagCameraType { SIMULATION_CAMERA(AprilTagSimulationCameraIO::new), LIMELIGHT(AprilTagLimelightIO::new); - final BiFunction createIOFunction; + final BiFunction createIOFunction; - AprilTagCameraType(BiFunction createIOFunction) { + AprilTagCameraType(BiFunction createIOFunction) { this.createIOFunction = createIOFunction; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index 8f3462d2..5625c2bd 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -1,18 +1,17 @@ package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import org.littletonrobotics.junction.AutoLog; import frc.trigon.lib.hardware.RobotHardwareStats; +import org.littletonrobotics.junction.AutoLog; public class AprilTagCameraIO { - static AprilTagCameraIO generateIO(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotToCamera) { + static AprilTagCameraIO generateIO(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, DynamicCameraTransform dynamicCameraTransform) { if (RobotHardwareStats.isReplay()) return new AprilTagCameraIO(); if (RobotHardwareStats.isSimulation()) aprilTagCameraType = AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA; - return aprilTagCameraType.createIOFunction.apply(name, robotToCamera); + return aprilTagCameraType.createIOFunction.apply(name, dynamicCameraTransform); } protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java new file mode 100644 index 00000000..29dbc9d9 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java @@ -0,0 +1,50 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.*; + +import java.util.function.Function; + +public class DynamicCameraTransform { + private final Function robotCenterToCameraFunction; + + public DynamicCameraTransform(Transform3d robotCenterToCamera) { + this(timestamp -> robotCenterToCamera); + } + + public DynamicCameraTransform(Function robotCenterToCameraFunction) { + this.robotCenterToCameraFunction = robotCenterToCameraFunction; + } + + public Pose2d calculate2dRobotPose(Pose2d cameraPose, double timestampSeconds) { + final Transform2d robotCenterToCamera = get2dRobotCenterToCamera(timestampSeconds); + return cameraPose.transformBy(robotCenterToCamera); + } + + public Pose3d calculate3dRobotPose(Pose3d cameraPose, double timestampSeconds) { + final Transform3d robotCenterToCamera = get3dRobotCenterToCamera(timestampSeconds); + return cameraPose.transformBy(robotCenterToCamera); + } + + public Transform2d get2dRobotCenterToCamera(double timestampSeconds) { + return toTransform2d(get3dRobotCenterToCamera(timestampSeconds)); + } + + public Transform2d get2dCameraToRobotCenter(double timestampSeconds) { + return toTransform2d(get3dCameraToRobotCenter(timestampSeconds)); + } + + public Transform3d get3dRobotCenterToCamera(double timestampSeconds) { + return robotCenterToCameraFunction.apply(timestampSeconds); + } + + public Transform3d get3dCameraToRobotCenter(double timestampSeconds) { + return get3dRobotCenterToCamera(timestampSeconds).inverse(); + } + + private Transform2d toTransform2d(Transform3d transform3d) { + final Translation2d robotCenterToCameraTranslation = transform3d.getTranslation().toTranslation2d(); + final Rotation2d robotCenterToCameraRotation = transform3d.getRotation().toRotation2d(); + + return new Transform2d(robotCenterToCameraTranslation, robotCenterToCameraRotation); + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 60a0e81b..0e03b7ac 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,16 +1,16 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.lib.utilities.LimelightHelpers; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; -import frc.trigon.lib.utilities.LimelightHelpers; +import frc.trigon.robot.poseestimation.apriltagcamera.DynamicCameraTransform; // TODO: Fully implement this class, Limelight currently not supported. public class AprilTagLimelightIO extends AprilTagCameraIO { private final String hostname; - public AprilTagLimelightIO(String hostname, Transform3d robotToCamera) { + public AprilTagLimelightIO(String hostname, DynamicCameraTransform dynamicCameraTransform) { this.hostname = hostname; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 138968e4..5722eb30 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -13,6 +13,7 @@ import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.apriltagcamera.DynamicCameraTransform; import org.photonvision.PhotonCamera; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; @@ -25,12 +26,12 @@ import java.util.Optional; public class AprilTagPhotonCameraIO extends AprilTagCameraIO { - private final Transform3d robotToCamera; + protected final DynamicCameraTransform dynamicCameraTransform; final PhotonCamera photonCamera; - public AprilTagPhotonCameraIO(String cameraName, Transform3d robotToCamera) { + public AprilTagPhotonCameraIO(String cameraName, DynamicCameraTransform dynamicCameraTransform) { photonCamera = new PhotonCamera(cameraName); - this.robotToCamera = robotToCamera; + this.dynamicCameraTransform = dynamicCameraTransform; } @Override @@ -138,8 +139,10 @@ private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pos if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) return null; - Pose3d fieldToRobotSeed = bestCameraSolvePNPPose.transformBy(this.robotToCamera.inverse()); - final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(result.getTimestampSeconds()).getRotation(); + final double resultTimestamp = result.getTimestampSeconds(); + final Transform3d robotCenterToCamera = dynamicCameraTransform.get3dRobotCenterToCamera(resultTimestamp); + Pose3d fieldToRobotSeed = bestCameraSolvePNPPose.transformBy(robotCenterToCamera.inverse()); + final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(resultTimestamp).getRotation(); if (!AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingFree()) { fieldToRobotSeed = new Pose3d( @@ -152,7 +155,7 @@ private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pos cameraMatrix.get(), distCoeffs.get(), result.getTargets(), - robotToCamera, + robotCenterToCamera, fieldToRobotSeed, FieldConstants.APRIL_TAG_FIELD_LAYOUT, TargetModel.kAprilTag36h11, @@ -161,7 +164,7 @@ private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pos AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingScaleFactor() ); - return pnpResult.map(value -> Pose3d.kZero.plus(value.best).transformBy(robotToCamera)).orElse(null); + return pnpResult.map(value -> Pose3d.kZero.plus(value.best).transformBy(robotCenterToCamera)).orElse(null); } private int[] getVisibleTagIDs(PhotonTrackedTarget[] targets) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java index c68ef0e0..4b6019c8 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -1,16 +1,25 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.Timer; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.apriltagcamera.DynamicCameraTransform; import org.photonvision.simulation.PhotonCameraSim; public class AprilTagSimulationCameraIO extends AprilTagPhotonCameraIO { + private final PhotonCameraSim cameraSimulation; - public AprilTagSimulationCameraIO(String cameraName, Transform3d robotToCamera) { - super(cameraName, robotToCamera); + public AprilTagSimulationCameraIO(String cameraName, DynamicCameraTransform dynamicCameraTransform) { + super(cameraName, dynamicCameraTransform); - final PhotonCameraSim cameraSimulation = new PhotonCameraSim(photonCamera, AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES); + cameraSimulation = new PhotonCameraSim(photonCamera, AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES); cameraSimulation.enableDrawWireframe(true); - AprilTagCameraConstants.VISION_SIMULATION.addCamera(cameraSimulation, robotToCamera); + AprilTagCameraConstants.VISION_SIMULATION.addCamera(cameraSimulation, dynamicCameraTransform.get3dRobotCenterToCamera(Timer.getFPGATimestamp())); + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + AprilTagCameraConstants.VISION_SIMULATION.adjustCamera(cameraSimulation, dynamicCameraTransform.get3dRobotCenterToCamera(Timer.getFPGATimestamp())); + super.updateInputs(inputs); } } \ No newline at end of file From b7b3ba75a335622c40ab805718ace77232154ce9 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Mon, 19 Jan 2026 18:05:23 +0200 Subject: [PATCH 2/9] Added JDocs --- .../apriltagcamera/AprilTagCamera.java | 19 +++--- .../DynamicCameraTransform.java | 62 ++++++++++++++++++- 2 files changed, 68 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 6f7797cc..d07a487d 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -24,16 +24,13 @@ public class AprilTagCamera { private Pose2d estimatedRobotPose = new Pose2d(); /** - * Constructs a new AprilTagCamera. + * Constructs a new AprilTagCamera with a static camera transform. * * @param aprilTagCameraType the type of camera * @param name the camera's name - * @param robotCenterToCamera the transform of the robot's origin point to the camera. - * only the x, y and yaw values will be used for transforming the camera pose to the robot's center, - * to avoid more inaccuracies like pitch and roll. - * The reset will be used for creating a camera in simulation + * @param robotCenterToCamera the static transform from robot center to camera * @param standardDeviations the initial calibrated standard deviations for the camera's estimated pose, - * will be changed as the distance from the tag(s) changes and the number of tags changes + * adjusted based on distance from tags and number of visible tags */ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotCenterToCamera, @@ -42,16 +39,14 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT } /** - * Constructs a new AprilTagCamera. + * Constructs a new AprilTagCamera with a dynamic camera transform. * * @param aprilTagCameraType the type of camera * @param name the camera's name - * @param dynamicCameraTransform the transform of the robot's origin point to the camera. - * only the x, y and yaw values will be used for transforming the camera pose to the robot's center, - * to avoid more inaccuracies like pitch and roll. - * The reset will be used for creating a camera in simulation + * @param dynamicCameraTransform the dynamic transform from robot center to camera, + * supports time-dependent camera positioning * @param standardDeviations the initial calibrated standard deviations for the camera's estimated pose, - * will be changed as the distance from the tag(s) changes and the number of tags changes + * adjusted based on distance from tags and number of visible tags */ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, DynamicCameraTransform dynamicCameraTransform, diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java index 29dbc9d9..1b12da16 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java @@ -4,47 +4,107 @@ import java.util.function.Function; +/** + * Handles transformations between camera and robot coordinate frames. + * Supports both static and time-dependent camera positions, useful for cameras on moving mechanisms. + */ public class DynamicCameraTransform { private final Function robotCenterToCameraFunction; + /** + * Constructs a DynamicCameraTransform with a static camera position. + * + * @param robotCenterToCamera the fixed transform from robot center to camera + */ public DynamicCameraTransform(Transform3d robotCenterToCamera) { this(timestamp -> robotCenterToCamera); } + /** + * Constructs a DynamicCameraTransform with a time-dependent camera position. + * + * @param robotCenterToCameraFunction function that returns the transform from robot center to camera at a given timestamp + */ public DynamicCameraTransform(Function robotCenterToCameraFunction) { this.robotCenterToCameraFunction = robotCenterToCameraFunction; } + /** + * Transforms a camera pose to a robot pose in 2D. + * + * @param cameraPose the camera's pose on the field + * @param timestampSeconds the timestamp for the camera transform + * @return the robot's pose on the field + */ public Pose2d calculate2dRobotPose(Pose2d cameraPose, double timestampSeconds) { final Transform2d robotCenterToCamera = get2dRobotCenterToCamera(timestampSeconds); return cameraPose.transformBy(robotCenterToCamera); } + /** + * Transforms a camera pose to a robot pose in 3D. + * + * @param cameraPose the camera's pose in 3D space + * @param timestampSeconds the timestamp for the camera transform + * @return the robot's pose in 3D space + */ public Pose3d calculate3dRobotPose(Pose3d cameraPose, double timestampSeconds) { final Transform3d robotCenterToCamera = get3dRobotCenterToCamera(timestampSeconds); return cameraPose.transformBy(robotCenterToCamera); } + /** + * Gets the 2D transform from robot center to camera. + * Only x, y, and yaw components are preserved to avoid pitch and roll inaccuracies. + * + * @param timestampSeconds the timestamp for the transform + * @return the 2D transform from robot center to camera + */ public Transform2d get2dRobotCenterToCamera(double timestampSeconds) { return toTransform2d(get3dRobotCenterToCamera(timestampSeconds)); } + /** + * Gets the 2D transform from camera to robot center. + * Only x, y, and yaw components are preserved to avoid pitch and roll inaccuracies. + * + * @param timestampSeconds the timestamp for the transform + * @return the 2D transform from camera to robot center + */ public Transform2d get2dCameraToRobotCenter(double timestampSeconds) { return toTransform2d(get3dCameraToRobotCenter(timestampSeconds)); } + /** + * Gets the 3D transform from robot center to camera. + * + * @param timestampSeconds the timestamp for the transform + * @return the 3D transform from robot center to camera + */ public Transform3d get3dRobotCenterToCamera(double timestampSeconds) { return robotCenterToCameraFunction.apply(timestampSeconds); } + /** + * Gets the 3D transform from camera to robot center. + * + * @param timestampSeconds the timestamp for the transform + * @return the 3D transform from camera to robot center + */ public Transform3d get3dCameraToRobotCenter(double timestampSeconds) { return get3dRobotCenterToCamera(timestampSeconds).inverse(); } + /** + * Converts a 3D transform to a 2D transform using only x, y, and yaw components. + * + * @param transform3d the 3D transform to convert + * @return the 2D transform + */ private Transform2d toTransform2d(Transform3d transform3d) { final Translation2d robotCenterToCameraTranslation = transform3d.getTranslation().toTranslation2d(); final Rotation2d robotCenterToCameraRotation = transform3d.getRotation().toRotation2d(); return new Transform2d(robotCenterToCameraTranslation, robotCenterToCameraRotation); } -} +} \ No newline at end of file From 7a719f1777260ebb67632d9861c78a38a77f6af1 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Mon, 19 Jan 2026 18:20:49 +0200 Subject: [PATCH 3/9] Correct transform logic --- .../apriltagcamera/DynamicCameraTransform.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java index 1b12da16..d0e8f831 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java @@ -37,8 +37,8 @@ public DynamicCameraTransform(Function robotCenterToCameraF * @return the robot's pose on the field */ public Pose2d calculate2dRobotPose(Pose2d cameraPose, double timestampSeconds) { - final Transform2d robotCenterToCamera = get2dRobotCenterToCamera(timestampSeconds); - return cameraPose.transformBy(robotCenterToCamera); + final Transform2d cameraToRobotCenter = get2dCameraToRobotCenter(timestampSeconds); + return cameraPose.transformBy(cameraToRobotCenter); } /** @@ -49,8 +49,8 @@ public Pose2d calculate2dRobotPose(Pose2d cameraPose, double timestampSeconds) { * @return the robot's pose in 3D space */ public Pose3d calculate3dRobotPose(Pose3d cameraPose, double timestampSeconds) { - final Transform3d robotCenterToCamera = get3dRobotCenterToCamera(timestampSeconds); - return cameraPose.transformBy(robotCenterToCamera); + final Transform3d cameraToRobotCenter = get3dCameraToRobotCenter(timestampSeconds); + return cameraPose.transformBy(cameraToRobotCenter); } /** From 2d65347e1b433004ff45c2d1bdabb8da65adf590 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 26 Jan 2026 19:42:06 +0200 Subject: [PATCH 4/9] Update lib --- src/main/java/frc/trigon/lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 649ab1b6..9403200d 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 649ab1b6e86077682ddef325b9355bb5547ea14f +Subproject commit 9403200d5d1161a2c75be560251618af01230b5a From 77310752cef9cb3d9fc876352f8218d301217540 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 26 Jan 2026 20:24:38 +0200 Subject: [PATCH 5/9] added calculation for turret camera pose --- .../trigon/robot/constants/CameraConstants.java | 12 ++++++++++++ .../robot/misc/TurretCameraCalculations.java | 16 ++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 src/main/java/frc/trigon/robot/misc/TurretCameraCalculations.java diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 40fae989..b68fffdc 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -4,7 +4,12 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import frc.trigon.robot.misc.TurretCameraCalculations; import frc.trigon.robot.misc.objectdetection.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.DynamicCameraTransform; +import frc.trigon.robot.poseestimation.robotposeestimator.StandardDeviations; public class CameraConstants { private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d(//TODO: AHAHAHAHAH @@ -12,9 +17,16 @@ public class CameraConstants { new Rotation3d(0, Units.degreesToRadians(30), 0) ); + private static final DynamicCameraTransform ROBOT_CENTER_TO_HUB_TAG_CAMERA = new DynamicCameraTransform(timeStamp -> TurretCameraCalculations.CAMERA_TO_ROBOT()); + public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 0.5; public static final ObjectDetectionCamera OBJECT_DETECTION_CAMERA = new ObjectDetectionCamera( "ObjectDetectionCamera", ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA ); + + public static final AprilTagCamera HUB_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "HubTagCamera", ROBOT_CENTER_TO_HUB_TAG_CAMERA, + new StandardDeviations(1, 1)); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/TurretCameraCalculations.java b/src/main/java/frc/trigon/robot/misc/TurretCameraCalculations.java new file mode 100644 index 00000000..73dfbcdc --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/TurretCameraCalculations.java @@ -0,0 +1,16 @@ +package frc.trigon.robot.misc; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import frc.trigon.robot.RobotContainer; + +public class TurretCameraCalculations { + private static final Transform3d CAMERA_TO_TURRET = new Transform3d(); + private static final Translation3d TURRET_TO_ROBOT = new Translation3d(); + + public static Transform3d CAMERA_TO_ROBOT() { + return CAMERA_TO_TURRET.plus(new Transform3d(TURRET_TO_ROBOT, + new Rotation3d(RobotContainer.TURRET.getCurrentSelfRelativeAngle()))); + } +} \ No newline at end of file From 7c5afae9576a1e1218f9b0ea95c584277c9f67b8 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Tue, 27 Jan 2026 23:34:23 +0200 Subject: [PATCH 6/9] No need to update lib, thread the signal --- src/main/java/frc/trigon/lib | 2 +- .../frc/trigon/robot/subsystems/turret/TurretConstants.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 9403200d..6b6c7adb 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 9403200d5d1161a2c75be560251618af01230b5a +Subproject commit 6b6c7adbf9d1111fa67fdc0b801e0520044bc7f8 diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java index 91f0666d..3dda0209 100644 --- a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java @@ -108,7 +108,7 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MASTER_MOTOR.registerThreadedSignal(TalonFXSignal.POSITION, 250); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); } @@ -143,7 +143,7 @@ private static void configureEncoder() { ENCODER.applyConfiguration(config); ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); - ENCODER.registerSignal(CANcoderSignal.POSITION, 100); + ENCODER.registerSignal(CANcoderSignal.POSITION, 250); ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); } } \ No newline at end of file From 12e6170c51b2096eb7c948606401ba78bc6075c4 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Wed, 28 Jan 2026 00:46:59 +0200 Subject: [PATCH 7/9] Implement cleanly --- .../java/frc/trigon/robot/RobotContainer.java | 5 +- .../robot/constants/CameraConstants.java | 26 ++++-- .../robot/misc/TurretCameraCalculations.java | 16 ---- .../io/AprilTagPhotonCameraIO.java | 8 +- .../robot/subsystems/turret/Turret.java | 7 ++ .../TurretCameraTransformCalculator.java | 83 +++++++++++++++++++ .../subsystems/turret/TurretConstants.java | 26 ++++-- 7 files changed, 136 insertions(+), 35 deletions(-) delete mode 100644 src/main/java/frc/trigon/robot/misc/TurretCameraCalculations.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index a0d3fe81..697a97ca 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -40,7 +40,10 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { - public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator(); + public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator( + CameraConstants.RIGHT_TURRET_CAMERA, + CameraConstants.LEFT_TURRET_CAMERA + ); public static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = new ObjectPoseEstimator( CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS, SimulatedGamePieceConstants.GamePieceType.FUEL, diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index b68fffdc..b954c60e 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -4,29 +4,41 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import frc.trigon.robot.misc.TurretCameraCalculations; import frc.trigon.robot.misc.objectdetection.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.DynamicCameraTransform; import frc.trigon.robot.poseestimation.robotposeestimator.StandardDeviations; +import frc.trigon.robot.subsystems.turret.TurretCameraTransformCalculator; public class CameraConstants { - private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d(//TODO: AHAHAHAHAH + private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d(//TODO: Find actual values new Translation3d(0, 0, 0.8), new Rotation3d(0, Units.degreesToRadians(30), 0) ); - private static final DynamicCameraTransform ROBOT_CENTER_TO_HUB_TAG_CAMERA = new DynamicCameraTransform(timeStamp -> TurretCameraCalculations.CAMERA_TO_ROBOT()); - public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 0.5; public static final ObjectDetectionCamera OBJECT_DETECTION_CAMERA = new ObjectDetectionCamera( "ObjectDetectionCamera", ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA ); - public static final AprilTagCamera HUB_CAMERA = new AprilTagCamera( + private static final StandardDeviations APRIL_TAG_CAMERA_STANDARD_DEVIATIONS = new StandardDeviations( + 0.015, + 0.01 + ); + public static final AprilTagCamera + RIGHT_TURRET_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "HubTagCamera", ROBOT_CENTER_TO_HUB_TAG_CAMERA, - new StandardDeviations(1, 1)); + "RightTurretCamera", + new DynamicCameraTransform(TurretCameraTransformCalculator.getInstance()::calculateRobotToRightCameraAtTime), + APRIL_TAG_CAMERA_STANDARD_DEVIATIONS + ), + LEFT_TURRET_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "LeftTurretCamera", + new DynamicCameraTransform(TurretCameraTransformCalculator.getInstance()::calculateRobotToLeftCameraAtTime), + APRIL_TAG_CAMERA_STANDARD_DEVIATIONS + ); + } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/TurretCameraCalculations.java b/src/main/java/frc/trigon/robot/misc/TurretCameraCalculations.java deleted file mode 100644 index 73dfbcdc..00000000 --- a/src/main/java/frc/trigon/robot/misc/TurretCameraCalculations.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.trigon.robot.misc; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import frc.trigon.robot.RobotContainer; - -public class TurretCameraCalculations { - private static final Transform3d CAMERA_TO_TURRET = new Transform3d(); - private static final Translation3d TURRET_TO_ROBOT = new Translation3d(); - - public static Transform3d CAMERA_TO_ROBOT() { - return CAMERA_TO_TURRET.plus(new Transform3d(TURRET_TO_ROBOT, - new Rotation3d(RobotContainer.TURRET.getCurrentSelfRelativeAngle()))); - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 5722eb30..2db97798 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -141,12 +141,12 @@ private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pos final double resultTimestamp = result.getTimestampSeconds(); final Transform3d robotCenterToCamera = dynamicCameraTransform.get3dRobotCenterToCamera(resultTimestamp); - Pose3d fieldToRobotSeed = bestCameraSolvePNPPose.transformBy(robotCenterToCamera.inverse()); + Pose3d fieldToRobotGuess = bestCameraSolvePNPPose.transformBy(robotCenterToCamera.inverse()); final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(resultTimestamp).getRotation(); if (!AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingFree()) { - fieldToRobotSeed = new Pose3d( - fieldToRobotSeed.getTranslation(), + fieldToRobotGuess = new Pose3d( + fieldToRobotGuess.getTranslation(), new Rotation3d(robotYawAtTimestamp) ); } @@ -156,7 +156,7 @@ private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pos distCoeffs.get(), result.getTargets(), robotCenterToCamera, - fieldToRobotSeed, + fieldToRobotGuess, FieldConstants.APRIL_TAG_FIELD_LAYOUT, TargetModel.kAprilTag36h11, AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingFree(), diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java index 2a52ef04..3025313c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java @@ -6,6 +6,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.hardware.phoenix6.Phoenix6SignalThread; import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderEncoder; import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderSignal; import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -61,6 +62,12 @@ public void updatePeriodically() { masterMotor.update(); followerMotor.update(); encoder.update(); + + TurretCameraTransformCalculator.getInstance().update( + masterMotor.getThreadedSignal(TalonFXSignal.POSITION), + Phoenix6SignalThread.getInstance().getLatestTimestamps(), + masterMotor.getSignal(TalonFXSignal.VELOCITY) + ); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java new file mode 100644 index 00000000..cf78fd1e --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java @@ -0,0 +1,83 @@ +package frc.trigon.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; + +import java.util.Map; + +public class TurretCameraTransformCalculator { + private static TurretCameraTransformCalculator INSTANCE = null; + private final TimeInterpolatableBuffer turretAngleBuffer = TimeInterpolatableBuffer.createBuffer(Rotation2d::interpolate, TurretConstants.TURRET_ANGLE_HISTORY_SIZE_SECONDS); + private double latestVelocityRotationsPerSecond = 0.0; + + public static TurretCameraTransformCalculator getInstance() { + if (INSTANCE == null) + INSTANCE = new TurretCameraTransformCalculator(); + return INSTANCE; + } + + private TurretCameraTransformCalculator() { + } + + public void update(double[] turretPositions, double[] timestamps, double velocityRotationsPerSecond) { + for (int i = 0; i < turretPositions.length; i++) + addSample(turretPositions[i], timestamps[i]); + this.latestVelocityRotationsPerSecond = velocityRotationsPerSecond; + } + + public Transform3d calculateRobotToRightCameraAtTime(double timestampSeconds) { + return calculateRobotToCameraAtTime(timestampSeconds, TurretConstants.TURRET_TO_RIGHT_CAMERA_TRANSFORM); + } + + public Transform3d calculateRobotToLeftCameraAtTime(double timestampSeconds) { + return calculateRobotToCameraAtTime(timestampSeconds, TurretConstants.TURRET_TO_LEFT_CAMERA_TRANSFORM); + } + + private Transform3d calculateRobotToCameraAtTime(double timestampSeconds, Transform3d turretToCameraTransform) { + final Rotation2d turretAngle = calculateTurretAngleAtTime(timestampSeconds); + if (turretAngle == null) + return null; + + final Transform3d turretAngleTransform = new Transform3d( + new Translation3d(), + new Rotation3d(0, 0, turretAngle.getRadians()) + ); + final Pose3d rotatedTurretOrigin = TurretConstants.TURRET_VISUALIZATION_ORIGIN_POINT.plus(turretAngleTransform); + final Pose3d cameraPose = rotatedTurretOrigin.plus(turretToCameraTransform); + return cameraPose.minus(new Pose3d()); + } + + private Rotation2d calculateTurretAngleAtTime(double timestampSeconds) { + if (isTimestampTooNew(timestampSeconds)) + return estimateFutureTurretAngle(timestampSeconds); + + return sampleTurretAngleAtTime(timestampSeconds); + } + + private boolean isTimestampTooNew(double timestampSeconds) { + final Double latestTimestamp = getLatestBufferEntry().getKey(); + return timestampSeconds > latestTimestamp; + } + + private Rotation2d estimateFutureTurretAngle(double futureTimestampSeconds) { + final Map.Entry latestBufferEntry = getLatestBufferEntry(); + final Double latestTimestamp = latestBufferEntry.getKey(); + final Rotation2d latestAngle = latestBufferEntry.getValue(); + + final double timeDeltaSeconds = futureTimestampSeconds - latestTimestamp; + final double predictedRotations = latestVelocityRotationsPerSecond * timeDeltaSeconds; + return latestAngle.plus(Rotation2d.fromRotations(predictedRotations)); + } + + private Rotation2d sampleTurretAngleAtTime(double timestampSeconds) { + return turretAngleBuffer.getSample(timestampSeconds).orElse(null); + } + + private Map.Entry getLatestBufferEntry() { + return turretAngleBuffer.getInternalBuffer().lastEntry(); + } + + private void addSample(double turretPositionRotations, double timestampSeconds) { + turretAngleBuffer.addSample(timestampSeconds, Rotation2d.fromRotations(turretPositionRotations)); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java index 1292aa9c..cd3a45a8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java @@ -4,10 +4,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.signals.*; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.util.Color; @@ -72,6 +69,21 @@ public class TurretConstants { TOTAL_ANGULAR_RANGE = MAXIMUM_ANGLE.minus(MINIMUM_ANGLE); static final double ROBOT_VELOCITY_TO_FUTURE_ANGLE_SECONDS = 0.2; + static final double TURRET_ANGLE_HISTORY_SIZE_SECONDS = 2; + static final Pose3d TURRET_ORIGIN_POINT_FOR_CAMERA_CALCULATION = new Pose3d( + new Translation3d(-0.14542, 0.14542, 0.34578), + new Rotation3d(0, 0, 0) + ); + static final Transform3d + TURRET_TO_RIGHT_CAMERA_TRANSFORM = new Transform3d( + new Translation3d(0.03, -0.05, 0.0), + new Rotation3d(0, 0, 0) + ), + TURRET_TO_LEFT_CAMERA_TRANSFORM = new Transform3d( + new Translation3d(0.03, 0.05, 0.0), + new Rotation3d(0, 0, 0) + ); + static { configureMasterMotor(); configureFollowerMotor(); @@ -120,9 +132,9 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); - MASTER_MOTOR.registerThreadedSignal(TalonFXSignal.POSITION, 250); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 250); + MASTER_MOTOR.registerThreadedSignal(TalonFXSignal.POSITION, 250); } private static void configureFollowerMotor() { @@ -157,6 +169,6 @@ private static void configureEncoder() { ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); ENCODER.registerSignal(CANcoderSignal.POSITION, 250); - ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); + ENCODER.registerSignal(CANcoderSignal.VELOCITY, 250); } } \ No newline at end of file From 8783fac66f15623da3fa8dce415863514f27262b Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Fri, 30 Jan 2026 00:03:00 +0200 Subject: [PATCH 8/9] Null checks and correction --- .../turret/TurretCameraTransformCalculator.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java index cf78fd1e..15179ae4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import frc.trigon.robot.RobotContainer; import java.util.Map; @@ -42,7 +43,7 @@ private Transform3d calculateRobotToCameraAtTime(double timestampSeconds, Transf new Translation3d(), new Rotation3d(0, 0, turretAngle.getRadians()) ); - final Pose3d rotatedTurretOrigin = TurretConstants.TURRET_VISUALIZATION_ORIGIN_POINT.plus(turretAngleTransform); + final Pose3d rotatedTurretOrigin = TurretConstants.TURRET_ORIGIN_POINT_FOR_CAMERA_CALCULATION.plus(turretAngleTransform); final Pose3d cameraPose = rotatedTurretOrigin.plus(turretToCameraTransform); return cameraPose.minus(new Pose3d()); } @@ -55,12 +56,17 @@ private Rotation2d calculateTurretAngleAtTime(double timestampSeconds) { } private boolean isTimestampTooNew(double timestampSeconds) { - final Double latestTimestamp = getLatestBufferEntry().getKey(); + final Map.Entry latestBufferEntry = getLatestBufferEntry(); + if (latestBufferEntry == null) + return false; + final Double latestTimestamp = latestBufferEntry.getKey(); return timestampSeconds > latestTimestamp; } private Rotation2d estimateFutureTurretAngle(double futureTimestampSeconds) { final Map.Entry latestBufferEntry = getLatestBufferEntry(); + if (latestBufferEntry == null) + return RobotContainer.TURRET.getCurrentSelfRelativeAngle(); final Double latestTimestamp = latestBufferEntry.getKey(); final Rotation2d latestAngle = latestBufferEntry.getValue(); From ca38c577514662e59d928b17d86a54531ea4177b Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Fri, 30 Jan 2026 01:24:30 +0200 Subject: [PATCH 9/9] added if --- .../subsystems/turret/TurretCameraTransformCalculator.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java index 15179ae4..1bc4cf8e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java @@ -21,6 +21,11 @@ private TurretCameraTransformCalculator() { } public void update(double[] turretPositions, double[] timestamps, double velocityRotationsPerSecond) { + if (turretPositions.length > timestamps.length) { + System.out.println("Turret positions and timestamps arrays must have the same length."); + return; + } + for (int i = 0; i < turretPositions.length; i++) addSample(turretPositions[i], timestamps[i]); this.latestVelocityRotationsPerSecond = velocityRotationsPerSecond;