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 40fae989..b954c60e 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -5,9 +5,14 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; 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) ); @@ -17,4 +22,23 @@ public class CameraConstants { "ObjectDetectionCamera", ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA ); + + 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, + "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/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index c7689a5c..d07a487d 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,31 +18,44 @@ 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(); /** - * 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, StandardDeviations standardDeviations) { + this(aprilTagCameraType, name, new DynamicCameraTransform(robotCenterToCamera), standardDeviations); + } + + /** + * Constructs a new AprilTagCamera with a dynamic camera transform. + * + * @param aprilTagCameraType the type of camera + * @param name the camera's name + * @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, + * adjusted based on distance from tags and number of visible tags + */ + 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 +122,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 +142,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 +183,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..d0e8f831 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java @@ -0,0 +1,110 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.*; + +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 cameraToRobotCenter = get2dCameraToRobotCenter(timestampSeconds); + return cameraPose.transformBy(cameraToRobotCenter); + } + + /** + * 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 cameraToRobotCenter = get3dCameraToRobotCenter(timestampSeconds); + return cameraPose.transformBy(cameraToRobotCenter); + } + + /** + * 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 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..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 @@ -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,12 +139,14 @@ 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 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) ); } @@ -152,8 +155,8 @@ private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pos cameraMatrix.get(), distCoeffs.get(), result.getTargets(), - robotToCamera, - fieldToRobotSeed, + robotCenterToCamera, + fieldToRobotGuess, FieldConstants.APRIL_TAG_FIELD_LAYOUT, TargetModel.kAprilTag36h11, AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingFree(), @@ -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 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..1bc4cf8e --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCameraTransformCalculator.java @@ -0,0 +1,94 @@ +package frc.trigon.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import frc.trigon.robot.RobotContainer; + +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) { + 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; + } + + 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_ORIGIN_POINT_FOR_CAMERA_CALCULATION.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 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(); + + 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 67d32225..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.registerSignal(TalonFXSignal.POSITION, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 250); + MASTER_MOTOR.registerThreadedSignal(TalonFXSignal.POSITION, 250); } private static void configureFollowerMotor() { @@ -156,7 +168,7 @@ private static void configureEncoder() { ENCODER.applyConfiguration(config); ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); - ENCODER.registerSignal(CANcoderSignal.POSITION, 100); - ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); + ENCODER.registerSignal(CANcoderSignal.POSITION, 250); + ENCODER.registerSignal(CANcoderSignal.VELOCITY, 250); } } \ No newline at end of file