From f94dc1f9bb96a75b8f6d28ae72c5cbcf7110bd9e Mon Sep 17 00:00:00 2001 From: rys4590 Date: Mon, 16 Mar 2026 15:40:12 +0200 Subject: [PATCH 01/19] i hate my life --- .../java/frc/robot/vision/cameras/limelight/Limelight.java | 7 +++++++ 1 file changed, 7 insertions(+) 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 be1d26e33f..850f90449e 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -102,6 +102,13 @@ public void updateNeuralDetection() { } } + public void updateWhateverTheHellImDoingDetection(){ + //figure out how to get the result from the python (ny) + int ny = 900; + int nx = 900; + + } + public void updateColorDetection() { if (pipeline.isColorDetecting()) { colorDetections.clear(); From a5cd9743ae5eda1e956a6ddec430200750a0f702 Mon Sep 17 00:00:00 2001 From: rys4590 Date: Mon, 16 Mar 2026 16:26:42 +0200 Subject: [PATCH 02/19] huh-? --- src/main/java/frc/robot/Robot.java | 14 +++++++++--- .../vision/cameras/limelight/Limelight.java | 22 ++++++++++++++----- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7669a7f2b4..2e3f842fc1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,6 +8,8 @@ import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -138,7 +140,9 @@ public Robot() { new Translation3d(0.297, -0.143, 0.361), new Rotation3d(Math.toRadians(-0.18), Math.toRadians(27.38), Math.toRadians(-0.35)) ), - LimelightPipeline.APRIL_TAG + LimelightPipeline.APRIL_TAG, + Rotation2d.fromDegrees(82), + Rotation2d.fromDegrees(56.2) ); limelightFront.setMT1StdDevsCalculation( @@ -167,7 +171,9 @@ public Robot() { new Translation3d(-0.06, 0.367, 0.469), new Rotation3d(Math.toRadians(-177.78), Math.toRadians(20.64), Math.toRadians(-90.7)) ), - LimelightPipeline.APRIL_TAG + LimelightPipeline.APRIL_TAG, + Rotation2d.fromDegrees(82), + Rotation2d.fromDegrees(56.2) ); limelightRight.setMT1StdDevsCalculation( LimelightStdDevCalculations.getMT1StdDevsCalculation( @@ -195,7 +201,9 @@ public Robot() { new Translation3d(-0.077, -0.345, 0.5), new Rotation3d(Math.toRadians(-0.17), Math.toRadians(19.31), Math.toRadians(91.91)) ), - LimelightPipeline.APRIL_TAG + LimelightPipeline.APRIL_TAG, + Rotation2d.fromDegrees(82), + Rotation2d.fromDegrees(56.2) ); limelightLeft.setMT1StdDevsCalculation( LimelightStdDevCalculations.getMT1StdDevsCalculation( 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 850f90449e..eb4bc0d67f 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -30,6 +30,9 @@ public class Limelight implements ObjectDetector, IndependentRobotPoseSupplier, private final ArrayList neuralDetections; private final ArrayList colorDetections; + private final Rotation2d horizontalFOV; + private final Rotation2d verticalFOV; + private final LimelightInputsSet inputs; private RobotPoseObservation mt1PoseObservation; @@ -45,7 +48,7 @@ public class Limelight implements ObjectDetector, IndependentRobotPoseSupplier, private LimelightPipeline pipeline; - public Limelight(String name, String logPathPrefix, Pose3d robotRelativeCameraPose, LimelightPipeline pipeline) { + public Limelight(String name, String logPathPrefix, Pose3d robotRelativeCameraPose, LimelightPipeline pipeline, Rotation2d horizontalFOV, Rotation2d verticalFOV) { this.name = name; this.logPath = logPathPrefix + "/" + name; @@ -55,6 +58,9 @@ public Limelight(String name, String logPathPrefix, Pose3d robotRelativeCameraPo this.neuralDetections = new ArrayList<>(); this.colorDetections = new ArrayList<>(); + this.horizontalFOV = horizontalFOV; + this.verticalFOV = verticalFOV; + this.mt1PoseObservation = new RobotPoseObservation(); this.mt2PoseObservation = new RobotPoseObservation(); @@ -102,10 +108,16 @@ public void updateNeuralDetection() { } } - public void updateWhateverTheHellImDoingDetection(){ - //figure out how to get the result from the python (ny) - int ny = 900; - int nx = 900; + public void updateWhateverTheHellImDoingDetection() { + // figure out how to get the result from the python (ny) + int dy = 900; + int dx = 900; + int width = 1800; + int height = 1200; + + double tx = dx * (horizontalFOV.getDegrees()/width); //(in degrees) + double ty = dy* (verticalFOV.getDegrees()/width); + } From 5320f058a905771f7e584be5b3363cb5c512f038 Mon Sep 17 00:00:00 2001 From: rys4590 Date: Mon, 16 Mar 2026 16:57:44 +0200 Subject: [PATCH 03/19] huh-? --- .../vision/cameras/limelight/Limelight.java | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) 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 eb4bc0d67f..c9cfcedb36 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -110,14 +110,15 @@ public void updateNeuralDetection() { public void updateWhateverTheHellImDoingDetection() { // figure out how to get the result from the python (ny) - int dy = 900; - int dx = 900; - int width = 1800; - int height = 1200; - - double tx = dx * (horizontalFOV.getDegrees()/width); //(in degrees) - double ty = dy* (verticalFOV.getDegrees()/width); - + double ny = 900; + double nx = 900; + double Vpw = 2*Math.tan(horizontalFOV.getDegrees()/2); + double Vph = 2*Math.tan(verticalFOV.getDegrees()/2); + double X = nx * (Vpw/2); + double Y = ny * (Vph/2); + double tx = Math.atan2(X,1); + double ty = Math.atan2(Y,1); + } From d3022e94ae34e0336448acbeb80d1a6f91f7b02c Mon Sep 17 00:00:00 2001 From: rys4590 Date: Mon, 16 Mar 2026 17:13:21 +0200 Subject: [PATCH 04/19] huh-? --- .../frc/robot/vision/cameras/limelight/Limelight.java | 9 +++++++++ .../vision/cameras/limelight/ObjectDetectionMath.java | 7 +++++-- 2 files changed, 14 insertions(+), 2 deletions(-) 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 c9cfcedb36..920d1c1ecd 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -29,6 +29,7 @@ public class Limelight implements ObjectDetector, IndependentRobotPoseSupplier, private final ArrayList neuralDetections; private final ArrayList colorDetections; + private final ArrayList weirdObjectThing; private final Rotation2d horizontalFOV; private final Rotation2d verticalFOV; @@ -57,6 +58,7 @@ public Limelight(String name, String logPathPrefix, Pose3d robotRelativeCameraPo this.neuralDetections = new ArrayList<>(); this.colorDetections = new ArrayList<>(); + this.weirdObjectThing = new ArrayList<>(); this.horizontalFOV = horizontalFOV; this.verticalFOV = verticalFOV; @@ -119,7 +121,14 @@ public void updateWhateverTheHellImDoingDetection() { double tx = Math.atan2(X,1); double ty = Math.atan2(Y,1); + Rotation2d yawOffset = Rotation2d.fromDegrees(tx); + Rotation2d pitchOffset = Rotation2d.fromDegrees(ty); + double objectRelativeToCameraX = ObjectDetectionMath.getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); + double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, yawOffset, objectRelativeToCameraX); + Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); + Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); + //need to make it compatibale with array but first we need ot see if we eevn have an array to return... } public void updateColorDetection() { diff --git a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java index 74b194dcde..f1c9c68a22 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java @@ -9,6 +9,8 @@ public class ObjectDetectionMath { + public static double heightOfFuelToMiddleMeters = 0.075; + public static DetectedObjectObservation getDetectedObjectObservation( Pose3d cameraPose, DetectedObjectType objectType, @@ -37,13 +39,14 @@ public static DetectedObjectObservation getDetectedObjectObservation( return new DetectedObjectObservation(objectType, robotRelativeObjectTranslation, timestampSeconds); } - private static double getCameraRelativeObjectX(Pose3d cameraPose, double objectCenterHeightMeters, Rotation2d objectToCrosshairPitchOffset) { + public static double getCameraRelativeObjectX(Pose3d cameraPose, double objectCenterHeightMeters, Rotation2d objectToCrosshairPitchOffset) { double objectAndCameraHeightDifferenceMeters = objectCenterHeightMeters - cameraPose.getZ(); Rotation2d objectAndCameraTotalPitch = objectToCrosshairPitchOffset.plus(Rotation2d.fromRadians(cameraPose.getRotation().getY())); return objectAndCameraHeightDifferenceMeters / objectAndCameraTotalPitch.getTan(); } - private static double getCameraRelativeObjectY( + + public static double getCameraRelativeObjectY( Pose3d cameraPose, double objectCenterHeightMeters, Rotation2d objectToCrosshairYawOffset, From 04af104e32b971082eca0ebad618f596e05d51aa Mon Sep 17 00:00:00 2001 From: rys4590 Date: Mon, 16 Mar 2026 17:17:30 +0200 Subject: [PATCH 05/19] okay fixed a few things, should be smooth sailing from here. --- .../frc/robot/vision/cameras/limelight/Limelight.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 920d1c1ecd..cba4f01fe8 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -114,15 +114,15 @@ public void updateWhateverTheHellImDoingDetection() { // figure out how to get the result from the python (ny) double ny = 900; double nx = 900; - double Vpw = 2*Math.tan(horizontalFOV.getDegrees()/2); - double Vph = 2*Math.tan(verticalFOV.getDegrees()/2); + double Vpw = 2 * Math.tan(horizontalFOV.getRadians() / 2); + double Vph = 2 * Math.tan(verticalFOV.getRadians() / 2); double X = nx * (Vpw/2); double Y = ny * (Vph/2); double tx = Math.atan2(X,1); double ty = Math.atan2(Y,1); - Rotation2d yawOffset = Rotation2d.fromDegrees(tx); - Rotation2d pitchOffset = Rotation2d.fromDegrees(ty); + Rotation2d yawOffset = new Rotation2d(tx); + Rotation2d pitchOffset = new Rotation2d(ty); double objectRelativeToCameraX = ObjectDetectionMath.getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, yawOffset, objectRelativeToCameraX); From 02b29e9a2c5425d7117fb9799aebfdc605ff276d Mon Sep 17 00:00:00 2001 From: rys4590 Date: Thu, 19 Mar 2026 18:02:05 +0200 Subject: [PATCH 06/19] okay fixed a few things, should be smooth sailing from here. --- .../robot/vision/cameras/limelight/Limelight.java | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) 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 cba4f01fe8..ae8dd7858b 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -1,6 +1,8 @@ package frc.robot.vision.cameras.limelight; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.vision.DetectedObjectObservation; import frc.robot.vision.RobotPoseObservation; import frc.robot.vision.cameras.limelight.inputs.LimelightInputsSet; @@ -112,8 +114,13 @@ public void updateNeuralDetection() { public void updateWhateverTheHellImDoingDetection() { // figure out how to get the result from the python (ny) - double ny = 900; - double nx = 900; + + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + + double[] llpython = table.getEntry("llpython").getDoubleArray(new double[8]); + + double nx = llpython[1]; + double ny = llpython[2]; double Vpw = 2 * Math.tan(horizontalFOV.getRadians() / 2); double Vph = 2 * Math.tan(verticalFOV.getRadians() / 2); double X = nx * (Vpw/2); @@ -123,11 +130,11 @@ public void updateWhateverTheHellImDoingDetection() { Rotation2d yawOffset = new Rotation2d(tx); Rotation2d pitchOffset = new Rotation2d(ty); - double objectRelativeToCameraX = ObjectDetectionMath.getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, yawOffset, objectRelativeToCameraX); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); + //need to make it compatibale with array but first we need ot see if we eevn have an array to return... } From 9926495eaa53d48bfdefacc7f89fc6e066e3d9f8 Mon Sep 17 00:00:00 2001 From: rys4590 Date: Thu, 19 Mar 2026 18:50:06 +0200 Subject: [PATCH 07/19] checking smth --- .../java/frc/robot/vision/cameras/limelight/Limelight.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 ae8dd7858b..e67984144c 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -112,7 +112,7 @@ public void updateNeuralDetection() { } } - public void updateWhateverTheHellImDoingDetection() { + public void updateHeatMapObjectDetection() { // figure out how to get the result from the python (ny) NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); @@ -135,7 +135,7 @@ public void updateWhateverTheHellImDoingDetection() { Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); - //need to make it compatibale with array but first we need ot see if we eevn have an array to return... + Logger.recordOutput(logPath+"/objectDetection", objectRelativeToField); } public void updateColorDetection() { From 90d2461942763653cbfdd3ffae169e680f339db1 Mon Sep 17 00:00:00 2001 From: rys4590 Date: Thu, 19 Mar 2026 18:54:34 +0200 Subject: [PATCH 08/19] sA --- src/main/java/frc/robot/Robot.java | 5 ++-- .../vision/cameras/limelight/Limelight.java | 29 ++++++++++++++----- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2e3f842fc1..c4ea4da0cb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,8 +8,6 @@ import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.AngleUnit; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -195,7 +193,7 @@ public Robot() { ); this.limelightLeft = new Limelight( - "limelight-left", + "limelight-two", "Vision", new Pose3d( new Translation3d(-0.077, -0.345, 0.5), @@ -285,6 +283,7 @@ public void periodic() { limelightFront.updateMT1(); limelightRight.updateMT1(); limelightLeft.updateMT1(); + limelightLeft.updateHeatMapObjectDetection(); limelightFront.getIndependentRobotPose().ifPresent(poseEstimator::updateVision); limelightRight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision); 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 e67984144c..f63e476e06 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -51,7 +51,14 @@ public class Limelight implements ObjectDetector, IndependentRobotPoseSupplier, private LimelightPipeline pipeline; - public Limelight(String name, String logPathPrefix, Pose3d robotRelativeCameraPose, LimelightPipeline pipeline, Rotation2d horizontalFOV, Rotation2d verticalFOV) { + public Limelight( + String name, + String logPathPrefix, + Pose3d robotRelativeCameraPose, + LimelightPipeline pipeline, + Rotation2d horizontalFOV, + Rotation2d verticalFOV + ) { this.name = name; this.logPath = logPathPrefix + "/" + name; @@ -123,19 +130,25 @@ public void updateHeatMapObjectDetection() { double ny = llpython[2]; double Vpw = 2 * Math.tan(horizontalFOV.getRadians() / 2); double Vph = 2 * Math.tan(verticalFOV.getRadians() / 2); - double X = nx * (Vpw/2); - double Y = ny * (Vph/2); - double tx = Math.atan2(X,1); - double ty = Math.atan2(Y,1); + double X = nx * (Vpw / 2); + double Y = ny * (Vph / 2); + double tx = Math.atan2(X, 1); + double ty = Math.atan2(Y, 1); Rotation2d yawOffset = new Rotation2d(tx); Rotation2d pitchOffset = new Rotation2d(ty); - double objectRelativeToCameraX = ObjectDetectionMath.getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); - double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, yawOffset, objectRelativeToCameraX); + double objectRelativeToCameraX = ObjectDetectionMath + .getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); + double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY( + robotRelativeCameraPose, + ObjectDetectionMath.heightOfFuelToMiddleMeters, + yawOffset, + objectRelativeToCameraX + ); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); - Logger.recordOutput(logPath+"/objectDetection", objectRelativeToField); + Logger.recordOutput(logPath + "/objectDetection", objectRelativeToField); } public void updateColorDetection() { From 469b2b45d7d9af2ce62b0883f15f105111af653f Mon Sep 17 00:00:00 2001 From: rys4590 Date: Fri, 20 Mar 2026 18:02:43 +0200 Subject: [PATCH 09/19] i coked --- .../java/frc/robot/vision/cameras/limelight/Limelight.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 f63e476e06..4554dc0a41 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -122,7 +122,7 @@ public void updateNeuralDetection() { public void updateHeatMapObjectDetection() { // figure out how to get the result from the python (ny) - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTable table = NetworkTableInstance.getDefault().getTable(name); double[] llpython = table.getEntry("llpython").getDoubleArray(new double[8]); @@ -147,7 +147,7 @@ public void updateHeatMapObjectDetection() { ); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); - + Logger.recordOutput(logPath+"/inputsFromPython", llpython); Logger.recordOutput(logPath + "/objectDetection", objectRelativeToField); } From 7826605d54f05babbd135e68a5367077dae7174e Mon Sep 17 00:00:00 2001 From: rys4590 Date: Fri, 20 Mar 2026 19:42:04 +0200 Subject: [PATCH 10/19] i sadly did not cook --- .../frc/robot/vision/cameras/limelight/Limelight.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 4554dc0a41..968d7edb55 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -132,13 +132,12 @@ public void updateHeatMapObjectDetection() { double Vph = 2 * Math.tan(verticalFOV.getRadians() / 2); double X = nx * (Vpw / 2); double Y = ny * (Vph / 2); - double tx = Math.atan2(X, 1); - double ty = Math.atan2(Y, 1); + double tx = Math.atan(nx * Math.tan(horizontalFOV.getRadians() / 2)); + double ty = Math.atan(ny * Math.tan(verticalFOV.getDegrees() / 2)); Rotation2d yawOffset = new Rotation2d(tx); Rotation2d pitchOffset = new Rotation2d(ty); - double objectRelativeToCameraX = ObjectDetectionMath - .getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); + double objectRelativeToCameraX = ObjectDetectionMath.getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters , pitchOffset); //temp instead of the constant double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY( robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, @@ -146,6 +145,7 @@ public void updateHeatMapObjectDetection() { objectRelativeToCameraX ); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); + Logger.recordOutput(logPath+"/objectRelativeToCamera", objectRelativeToCamera); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); Logger.recordOutput(logPath+"/inputsFromPython", llpython); Logger.recordOutput(logPath + "/objectDetection", objectRelativeToField); From 8c0711ade4dac12be38b4f711f50fd7d87ed85f0 Mon Sep 17 00:00:00 2001 From: rys4590 Date: Sat, 21 Mar 2026 15:59:59 +0200 Subject: [PATCH 11/19] when your code works, do spotless! --- .../frc/robot/vision/cameras/limelight/Limelight.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 968d7edb55..3e03f33827 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -137,7 +137,9 @@ public void updateHeatMapObjectDetection() { Rotation2d yawOffset = new Rotation2d(tx); Rotation2d pitchOffset = new Rotation2d(ty); - double objectRelativeToCameraX = ObjectDetectionMath.getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters , pitchOffset); //temp instead of the constant + double objectRelativeToCameraX = ObjectDetectionMath + .getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); // temp instead of + // the constant double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY( robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, @@ -145,9 +147,9 @@ public void updateHeatMapObjectDetection() { objectRelativeToCameraX ); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); - Logger.recordOutput(logPath+"/objectRelativeToCamera", objectRelativeToCamera); + Logger.recordOutput(logPath + "/objectRelativeToCamera", objectRelativeToCamera); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); - Logger.recordOutput(logPath+"/inputsFromPython", llpython); + Logger.recordOutput(logPath + "/inputsFromPython", llpython); Logger.recordOutput(logPath + "/objectDetection", objectRelativeToField); } From 75097f539caa48d4ace5a9ee6c0b9719fac12adb Mon Sep 17 00:00:00 2001 From: rys4590 Date: Sat, 21 Mar 2026 19:48:24 +0200 Subject: [PATCH 12/19] kid it does not work --- .../java/frc/robot/vision/cameras/limelight/Limelight.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 3e03f33827..21e04f01bb 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -133,17 +133,17 @@ public void updateHeatMapObjectDetection() { double X = nx * (Vpw / 2); double Y = ny * (Vph / 2); double tx = Math.atan(nx * Math.tan(horizontalFOV.getRadians() / 2)); - double ty = Math.atan(ny * Math.tan(verticalFOV.getDegrees() / 2)); + double ty = Math.atan(ny * Math.tan(verticalFOV.getRadians() / 2)); Rotation2d yawOffset = new Rotation2d(tx); Rotation2d pitchOffset = new Rotation2d(ty); double objectRelativeToCameraX = ObjectDetectionMath - .getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); // temp instead of + .getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, yawOffset); // temp instead of // the constant double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY( robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, - yawOffset, + pitchOffset, objectRelativeToCameraX ); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); From 1e397c28613d977add10c0c1ab18c5a7c84f7cdf Mon Sep 17 00:00:00 2001 From: rys4590 Date: Sun, 22 Mar 2026 19:22:23 +0200 Subject: [PATCH 13/19] big overhaul: 22.3.26 19:22 --- src/main/java/frc/robot/Robot.java | 8 ++--- .../vision/cameras/limelight/Limelight.java | 36 ++++++------------- .../limelight/ObjectDetectionMath.java | 19 ++++++++++ 3 files changed, 34 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c4ea4da0cb..7ada3471a4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -196,12 +196,12 @@ public Robot() { "limelight-two", "Vision", new Pose3d( - new Translation3d(-0.077, -0.345, 0.5), - new Rotation3d(Math.toRadians(-0.17), Math.toRadians(19.31), Math.toRadians(91.91)) + new Translation3d(-0.255, 5.5, 0.18), + new Rotation3d(Math.toRadians(0), Math.toRadians(-11), Math.toRadians(4)) ), LimelightPipeline.APRIL_TAG, - Rotation2d.fromDegrees(82), - Rotation2d.fromDegrees(56.2) + Rotation2d.fromDegrees(62.5), + Rotation2d.fromDegrees(48.9) ); limelightLeft.setMT1StdDevsCalculation( LimelightStdDevCalculations.getMT1StdDevsCalculation( 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 21e04f01bb..44e0bf566c 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -23,6 +23,8 @@ import java.util.function.Function; import java.util.function.Supplier; +import static frc.robot.vision.cameras.limelight.ObjectDetectionMath.*; + public class Limelight implements ObjectDetector, IndependentRobotPoseSupplier, OrientationRequiringRobotPoseSupplier { private final String name; @@ -122,35 +124,19 @@ public void updateNeuralDetection() { public void updateHeatMapObjectDetection() { // figure out how to get the result from the python (ny) - NetworkTable table = NetworkTableInstance.getDefault().getTable(name); - - double[] llpython = table.getEntry("llpython").getDoubleArray(new double[8]); - - double nx = llpython[1]; - double ny = llpython[2]; - double Vpw = 2 * Math.tan(horizontalFOV.getRadians() / 2); - double Vph = 2 * Math.tan(verticalFOV.getRadians() / 2); - double X = nx * (Vpw / 2); - double Y = ny * (Vph / 2); - double tx = Math.atan(nx * Math.tan(horizontalFOV.getRadians() / 2)); - double ty = Math.atan(ny * Math.tan(verticalFOV.getRadians() / 2)); - - Rotation2d yawOffset = new Rotation2d(tx); - Rotation2d pitchOffset = new Rotation2d(ty); - double objectRelativeToCameraX = ObjectDetectionMath - .getCameraRelativeObjectX(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, yawOffset); // temp instead of - // the constant - double objectRelativeToCameraY = ObjectDetectionMath.getCameraRelativeObjectY( - robotRelativeCameraPose, - ObjectDetectionMath.heightOfFuelToMiddleMeters, - pitchOffset, - objectRelativeToCameraX - ); + double tx = LimelightHelpers.getTX(name); + double ty = LimelightHelpers.getTY(name); + + Rotation2d yawOffset = Rotation2d.fromDegrees(tx); + Rotation2d pitchOffset = Rotation2d.fromDegrees(ty); + double objectRelativeToCameraX = getCameraRelativeObjectXR(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); + double objectRelativeToCameraY = getCameraRelativeObjectYR(yawOffset, objectRelativeToCameraX); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); Logger.recordOutput(logPath + "/objectRelativeToCamera", objectRelativeToCamera); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); - Logger.recordOutput(logPath + "/inputsFromPython", llpython); Logger.recordOutput(logPath + "/objectDetection", objectRelativeToField); + Logger.recordOutput(logPath+"/tx", tx); + Logger.recordOutput(logPath+"/ty", ty); } public void updateColorDetection() { diff --git a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java index f1c9c68a22..13aa0f2fb0 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java @@ -45,6 +45,25 @@ public static double getCameraRelativeObjectX(Pose3d cameraPose, double objectCe return objectAndCameraHeightDifferenceMeters / objectAndCameraTotalPitch.getTan(); } + public static double getCameraRelativeObjectYR( + Rotation2d yawOffset, + double forwardDistance + ) { + return Math.tan(yawOffset.getRadians()) * forwardDistance; + } + + public static double getCameraRelativeObjectXR( + Pose3d cameraPose, + double objectCenterHeightMeters, + Rotation2d pitchOffset + ) { + double heightDiff = objectCenterHeightMeters - cameraPose.getZ(); + double cameraPitch = cameraPose.getRotation().getY(); + double totalPitch = cameraPitch + pitchOffset.getRadians(); + + return heightDiff / Math.tan(totalPitch); + } + public static double getCameraRelativeObjectY( Pose3d cameraPose, From 883c42839fecd89de842fe73425c64355588fb39 Mon Sep 17 00:00:00 2001 From: rys4590 Date: Wed, 17 Jun 2026 19:43:39 +0300 Subject: [PATCH 14/19] ahh merge --- src/main/java/frc/robot/vision/DetectedObjectType.java | 2 +- .../java/frc/robot/vision/cameras/limelight/Limelight.java | 6 +++--- .../robot/vision/cameras/limelight/ObjectDetectionMath.java | 2 -- src/main/python/objectdetection/SnapscriptDetectionCode.py | 0 4 files changed, 4 insertions(+), 6 deletions(-) create mode 100644 src/main/python/objectdetection/SnapscriptDetectionCode.py diff --git a/src/main/java/frc/robot/vision/DetectedObjectType.java b/src/main/java/frc/robot/vision/DetectedObjectType.java index d15b85e7fe..70c7b69010 100644 --- a/src/main/java/frc/robot/vision/DetectedObjectType.java +++ b/src/main/java/frc/robot/vision/DetectedObjectType.java @@ -3,7 +3,7 @@ import java.util.Optional; public enum DetectedObjectType { - ; + FUEL("Fuel", 0.075); private final String name; private final double centerHeightFromFloorMeters; 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 ee2e3eb4c6..6c6ac934de 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -4,6 +4,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.vision.DetectedObjectObservation; +import frc.robot.vision.DetectedObjectType; import frc.robot.vision.RobotPoseObservation; import frc.robot.vision.cameras.limelight.inputs.LimelightInputsSet; import frc.robot.vision.interfaces.ObjectDetector; @@ -129,15 +130,14 @@ public void updateNeuralDetection() { } public void updateHeatMapObjectDetection() { - // figure out how to get the result from the python (ny) double tx = LimelightHelpers.getTX(name); double ty = LimelightHelpers.getTY(name); Rotation2d yawOffset = Rotation2d.fromDegrees(tx); Rotation2d pitchOffset = Rotation2d.fromDegrees(ty); - double objectRelativeToCameraX = getCameraRelativeObjectXR(robotRelativeCameraPose, ObjectDetectionMath.heightOfFuelToMiddleMeters, pitchOffset); - double objectRelativeToCameraY = getCameraRelativeObjectYR(yawOffset, objectRelativeToCameraX); + double objectRelativeToCameraX = getCameraRelativeObjectX(robotRelativeCameraPose, DetectedObjectType.FUEL.getCenterHeightFromFloorMeters(), pitchOffset); + double objectRelativeToCameraY = getCameraRelativeObjectY(robotRelativeCameraPose, DetectedObjectType.FUEL.getCenterHeightFromFloorMeters(), yawOffset, objectRelativeToCameraX); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); Logger.recordOutput(logPath + "/objectRelativeToCamera", objectRelativeToCamera); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); diff --git a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java index 13aa0f2fb0..bb0d2b051a 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java @@ -9,8 +9,6 @@ public class ObjectDetectionMath { - public static double heightOfFuelToMiddleMeters = 0.075; - public static DetectedObjectObservation getDetectedObjectObservation( Pose3d cameraPose, DetectedObjectType objectType, diff --git a/src/main/python/objectdetection/SnapscriptDetectionCode.py b/src/main/python/objectdetection/SnapscriptDetectionCode.py new file mode 100644 index 0000000000..e69de29bb2 From 0ca9cfaa4d04b355e0787964a0ccfed05d3618af Mon Sep 17 00:00:00 2001 From: rys4590 Date: Wed, 17 Jun 2026 19:44:38 +0300 Subject: [PATCH 15/19] ahh merge --- .../SnapscriptDetectionCode.py | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) diff --git a/src/main/python/objectdetection/SnapscriptDetectionCode.py b/src/main/python/objectdetection/SnapscriptDetectionCode.py index e69de29bb2..eff51f31ca 100644 --- a/src/main/python/objectdetection/SnapscriptDetectionCode.py +++ b/src/main/python/objectdetection/SnapscriptDetectionCode.py @@ -0,0 +1,89 @@ +import cv2 +import numpy as np + +lower_yellow = np.array([20, 100, 100]) +upper_yellow = np.array([30, 255, 255]) #wrong values impact the accuracy a lot. PLS NEXT PROJECT MAKE GOOD CALIBARATION CODE IN PYTHON + +COLUMNS = 25 + +def count_yellow_in_cell(mask, x_start, x_end, y_start, y_end): + cell_mask = mask[y_start:y_end, x_start:x_end] + return np.sum(cell_mask) + +def runPipeline(image, llrobot): + height, width = image.shape[:2] + + start = np.float32([[0, 0], [width, 0], [0, height], [width, height]]) + top_y = (start[0][1] + start[1][1]) / 2 + bottom_y = (start[2][1] + start[3][1]) / 2 + xfactor = bottom_y - top_y + yfactor = -xfactor + + dsize = (int(width), int(height + xfactor)) + end = np.float32([ + [0, 0], + [width, 0], + [-yfactor/2, height + xfactor], + [width + yfactor/2, height + xfactor] + ]) + + matrix = cv2.getPerspectiveTransform(start, end) + inv_matrix = np.linalg.inv(matrix) + warped = cv2.warpPerspective(image, matrix, dsize, flags=cv2.INTER_LINEAR) + + heightAfter, widthAfter = warped.shape[:2] + + hsv_img = cv2.cvtColor(warped, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(hsv_img, lower_yellow, upper_yellow) + + rows = round(heightAfter / (widthAfter / COLUMNS)) + cell_w = int(widthAfter / COLUMNS) + cell_h = int(heightAfter / rows) + + most_concentrated = -1 + most_x, most_y = -1, -1 + + for i in range(rows): + for j in range(COLUMNS): + x_start = j * cell_w + x_end = (j + 1) * cell_w + y_start = i * cell_h + y_end = (i + 1) * cell_h + + avg = count_yellow_in_cell(mask, x_start, x_end, y_start, y_end) / (cell_h * cell_w) + percent = avg * 100 / 255 + + if percent > most_concentrated: + most_concentrated = percent + most_x = j + most_y = i + + middle_x = most_x * cell_w + cell_w // 2 + middle_y = most_y * cell_h + cell_h // 2 + + pt = np.array([[middle_x], [middle_y], [1]]) + orig_pt = inv_matrix @ pt + orig_pt = orig_pt.flatten() + + orig_x = int(orig_pt[0] / orig_pt[2]) + orig_y = int(orig_pt[1] / orig_pt[2]) + + nx = (orig_x - (width / 2)) / (width / 2) + ny = (orig_y - (height / 2)) / (height / 2) + + contour = np.array([ + [[orig_x - cell_w, orig_y - cell_h]], + [[orig_x + cell_w, orig_y - cell_h]], + [[orig_x + cell_w, orig_y + cell_h]], + [[orig_x - cell_w, orig_y + cell_h]] + ]) + + x, y, w, h = cv2.boundingRect(contour) + + cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 255), 2) + + llpython = [1, nx, ny, orig_x, orig_y, most_concentrated, 0, 0] + + print(f"Grid cell: ({most_x}, {most_y}) | Pixel: ({orig_x}, {orig_y}) | nx={nx:.3f}, ny={ny:.3f}") + + return contour, image, llpython \ No newline at end of file From 3b043cea4ffe24631c31797b34afd5db632d31ad Mon Sep 17 00:00:00 2001 From: rys4590 Date: Sun, 21 Jun 2026 16:33:56 +0300 Subject: [PATCH 16/19] bruh --- .../frc/robot/vision/DetectedObjectType.java | 1 + .../vision/cameras/limelight/Limelight.java | 20 ++++++++++++------- .../limelight/ObjectDetectionMath.java | 11 ++-------- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/vision/DetectedObjectType.java b/src/main/java/frc/robot/vision/DetectedObjectType.java index 70c7b69010..d9330c65c2 100644 --- a/src/main/java/frc/robot/vision/DetectedObjectType.java +++ b/src/main/java/frc/robot/vision/DetectedObjectType.java @@ -3,6 +3,7 @@ import java.util.Optional; public enum DetectedObjectType { + FUEL("Fuel", 0.075); private final String name; 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 6c6ac934de..bc6dd94f0b 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -1,8 +1,6 @@ package frc.robot.vision.cameras.limelight; import edu.wpi.first.math.geometry.*; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.vision.DetectedObjectObservation; import frc.robot.vision.DetectedObjectType; import frc.robot.vision.RobotPoseObservation; @@ -130,20 +128,28 @@ public void updateNeuralDetection() { } public void updateHeatMapObjectDetection() { - double tx = LimelightHelpers.getTX(name); double ty = LimelightHelpers.getTY(name); Rotation2d yawOffset = Rotation2d.fromDegrees(tx); Rotation2d pitchOffset = Rotation2d.fromDegrees(ty); - double objectRelativeToCameraX = getCameraRelativeObjectX(robotRelativeCameraPose, DetectedObjectType.FUEL.getCenterHeightFromFloorMeters(), pitchOffset); - double objectRelativeToCameraY = getCameraRelativeObjectY(robotRelativeCameraPose, DetectedObjectType.FUEL.getCenterHeightFromFloorMeters(), yawOffset, objectRelativeToCameraX); + double objectRelativeToCameraX = getCameraRelativeObjectX( + robotRelativeCameraPose, + DetectedObjectType.FUEL.getCenterHeightFromFloorMeters(), + pitchOffset + ); + double objectRelativeToCameraY = getCameraRelativeObjectY( + robotRelativeCameraPose, + DetectedObjectType.FUEL.getCenterHeightFromFloorMeters(), + yawOffset, + objectRelativeToCameraX + ); Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); Logger.recordOutput(logPath + "/objectRelativeToCamera", objectRelativeToCamera); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); Logger.recordOutput(logPath + "/objectDetection", objectRelativeToField); - Logger.recordOutput(logPath+"/tx", tx); - Logger.recordOutput(logPath+"/ty", ty); + Logger.recordOutput(logPath + "/tx", tx); + Logger.recordOutput(logPath + "/ty", ty); } public void updateColorDetection() { diff --git a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java index bb0d2b051a..6a4ec6eea6 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java @@ -43,18 +43,11 @@ public static double getCameraRelativeObjectX(Pose3d cameraPose, double objectCe return objectAndCameraHeightDifferenceMeters / objectAndCameraTotalPitch.getTan(); } - public static double getCameraRelativeObjectYR( - Rotation2d yawOffset, - double forwardDistance - ) { + public static double getCameraRelativeObjectYR(Rotation2d yawOffset, double forwardDistance) { return Math.tan(yawOffset.getRadians()) * forwardDistance; } - public static double getCameraRelativeObjectXR( - Pose3d cameraPose, - double objectCenterHeightMeters, - Rotation2d pitchOffset - ) { + public static double getCameraRelativeObjectXR(Pose3d cameraPose, double objectCenterHeightMeters, Rotation2d pitchOffset) { double heightDiff = objectCenterHeightMeters - cameraPose.getZ(); double cameraPitch = cameraPose.getRotation().getY(); double totalPitch = cameraPitch + pitchOffset.getRadians(); From 10b8bcd4323af86978b981b9244ff0c142aef28b Mon Sep 17 00:00:00 2001 From: rys4590 Date: Mon, 22 Jun 2026 19:19:57 +0300 Subject: [PATCH 17/19] 1.5 --- .../java/frc/robot/vision/cameras/limelight/Limelight.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 bc6dd94f0b..1563552a3a 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -28,6 +28,7 @@ public class Limelight implements ObjectDetector, IndependentRobotPoseSupplier, private static final int THROTTLE_ENABLE_VALUE = 200; private static final int THROTTLE_DISABLE_VALUE = 0; + private static final double THREE_FOURTHS_OBJECT_FACTOR = 1.5; private final String name; private final String logPath; @@ -135,12 +136,12 @@ public void updateHeatMapObjectDetection() { Rotation2d pitchOffset = Rotation2d.fromDegrees(ty); double objectRelativeToCameraX = getCameraRelativeObjectX( robotRelativeCameraPose, - DetectedObjectType.FUEL.getCenterHeightFromFloorMeters(), + DetectedObjectType.FUEL.getCenterHeightFromFloorMeters()*THREE_FOURTHS_OBJECT_FACTOR, pitchOffset ); double objectRelativeToCameraY = getCameraRelativeObjectY( robotRelativeCameraPose, - DetectedObjectType.FUEL.getCenterHeightFromFloorMeters(), + DetectedObjectType.FUEL.getCenterHeightFromFloorMeters()*THREE_FOURTHS_OBJECT_FACTOR, yawOffset, objectRelativeToCameraX ); From a240f12c4c4060bf7153e8da20974bb6fdf5580f Mon Sep 17 00:00:00 2001 From: rys4590 Date: Thu, 25 Jun 2026 16:50:08 +0300 Subject: [PATCH 18/19] uh its 25.6.26 if thats what ur wondering. i spotlessed!! --- .../java/frc/robot/vision/cameras/limelight/Limelight.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 1563552a3a..99b59726b4 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -136,12 +136,12 @@ public void updateHeatMapObjectDetection() { Rotation2d pitchOffset = Rotation2d.fromDegrees(ty); double objectRelativeToCameraX = getCameraRelativeObjectX( robotRelativeCameraPose, - DetectedObjectType.FUEL.getCenterHeightFromFloorMeters()*THREE_FOURTHS_OBJECT_FACTOR, + DetectedObjectType.FUEL.getCenterHeightFromFloorMeters() * THREE_FOURTHS_OBJECT_FACTOR, pitchOffset ); double objectRelativeToCameraY = getCameraRelativeObjectY( robotRelativeCameraPose, - DetectedObjectType.FUEL.getCenterHeightFromFloorMeters()*THREE_FOURTHS_OBJECT_FACTOR, + DetectedObjectType.FUEL.getCenterHeightFromFloorMeters() * THREE_FOURTHS_OBJECT_FACTOR, yawOffset, objectRelativeToCameraX ); From fa457f14aba508962cea74190f2fcc059e7564c4 Mon Sep 17 00:00:00 2001 From: rys4590 Date: Thu, 25 Jun 2026 17:02:02 +0300 Subject: [PATCH 19/19] weeeeeeeeeeeeeeeeee --- .../robot/vision/cameras/limelight/Limelight.java | 5 ++++- .../cameras/limelight/ObjectDetectionMath.java | 13 ------------- 2 files changed, 4 insertions(+), 14 deletions(-) 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 99b59726b4..d409d66f68 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java @@ -139,15 +139,18 @@ public void updateHeatMapObjectDetection() { DetectedObjectType.FUEL.getCenterHeightFromFloorMeters() * THREE_FOURTHS_OBJECT_FACTOR, pitchOffset ); + double objectRelativeToCameraY = getCameraRelativeObjectY( robotRelativeCameraPose, DetectedObjectType.FUEL.getCenterHeightFromFloorMeters() * THREE_FOURTHS_OBJECT_FACTOR, yawOffset, objectRelativeToCameraX ); + Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY); - Logger.recordOutput(logPath + "/objectRelativeToCamera", objectRelativeToCamera); Translation2d objectRelativeToField = objectRelativeToCamera.rotateBy(robotRelativeCameraPose.getRotation().toRotation2d()); + + Logger.recordOutput(logPath + "/objectRelativeToCamera", objectRelativeToCamera); Logger.recordOutput(logPath + "/objectDetection", objectRelativeToField); Logger.recordOutput(logPath + "/tx", tx); Logger.recordOutput(logPath + "/ty", ty); diff --git a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java index 6a4ec6eea6..7b71dbaa00 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java +++ b/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java @@ -43,19 +43,6 @@ public static double getCameraRelativeObjectX(Pose3d cameraPose, double objectCe return objectAndCameraHeightDifferenceMeters / objectAndCameraTotalPitch.getTan(); } - public static double getCameraRelativeObjectYR(Rotation2d yawOffset, double forwardDistance) { - return Math.tan(yawOffset.getRadians()) * forwardDistance; - } - - public static double getCameraRelativeObjectXR(Pose3d cameraPose, double objectCenterHeightMeters, Rotation2d pitchOffset) { - double heightDiff = objectCenterHeightMeters - cameraPose.getZ(); - double cameraPitch = cameraPose.getRotation().getY(); - double totalPitch = cameraPitch + pitchOffset.getRadians(); - - return heightDiff / Math.tan(totalPitch); - } - - public static double getCameraRelativeObjectY( Pose3d cameraPose, double objectCenterHeightMeters,