diff --git a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java index 80eda8e429..94850eefbe 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -212,6 +212,17 @@ public List getRobotRelativeColorDetections() { return new ArrayList<>(); } + public static Pose2d calculateRobotCenterRelativeToField( + Supplier limeLightPoseRelativeToRobotCenter, + Pose2d limeLightPoseRelativeToField + ) { + Transform2d camTransformRelativeToRobotCenter = new Transform2d( + new Pose2d(0, 0, Rotation2d.fromDegrees(0.0)), + limeLightPoseRelativeToRobotCenter.get() + ); + return (limeLightPoseRelativeToField.transformBy(camTransformRelativeToRobotCenter.inverse())); + } + public Function getNeuralDetectionFilter() { return neuralDetectionFilter; }