From 85c888d20294522588167b33e7f652b3cbbeb1b4 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Fri, 3 Apr 2026 18:45:21 -0400 Subject: [PATCH 1/5] Lemon Hunter Init --- .../lemon_hunter/LemonHunterConstants.java | 20 +++++++++++++++++++ .../lemon_hunter/LemonHunterPreferences.java | 5 +++++ .../lemon_hunter/LemonHunterSubsystem.java | 16 +++++++++++++++ 3 files changed, 41 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java create mode 100644 src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterPreferences.java create mode 100644 src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java new file mode 100644 index 0000000..6ac049d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.lemon_hunter; + +import org.photonvision.PhotonCamera; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; + +public class LemonHunterConstants { + + private LemonHunterConstants() {} + + //Camera Constants + public static final String lemonHunterCameraName = "LEMON-HUNTER"; + public static final PhotonCamera lemonHunterCamera = new PhotonCamera(lemonHunterCameraName); + + //Base Transform (when intake is pulled in), actual transform updating as intake moves + public static final Transform3d lemonHunterBaseTransform = new Transform3d( + new Translation3d(0.3429, 0, 0.692), + new Rotation3d(0, Math.toRadians(30), 0)); +} diff --git a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterPreferences.java b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterPreferences.java new file mode 100644 index 0000000..f866213 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterPreferences.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.lemon_hunter; + +public class LemonHunterPreferences { + private LemonHunterPreferences() {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java new file mode 100644 index 0000000..f494c86 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.lemon_hunter; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LemonHunterSubsystem extends SubsystemBase{ + + public LemonHunterSubsystem(){} + + @Override + public void periodic(){ + var lemonHunterResults = LemonHunterConstants.lemonHunterCamera.getAllUnreadResults(); + for(var result : lemonHunterResults){ + + } + } +} From 7ee51fbc2bad839d4ccadb0f8316595676cd09a9 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Fri, 3 Apr 2026 19:10:54 -0400 Subject: [PATCH 2/5] Code to adjust for the fact that the hunter moves --- .../frc/robot/statemachines/HunterState.java | 32 +++++++++++++++++++ .../lemon_hunter/LemonHunterConstants.java | 3 ++ .../lemon_hunter/LemonHunterSubsystem.java | 31 ++++++++++++++++-- 3 files changed, 64 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/statemachines/HunterState.java diff --git a/src/main/java/frc/robot/statemachines/HunterState.java b/src/main/java/frc/robot/statemachines/HunterState.java new file mode 100644 index 0000000..531e1b6 --- /dev/null +++ b/src/main/java/frc/robot/statemachines/HunterState.java @@ -0,0 +1,32 @@ +package frc.robot.statemachines; + +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.units.measure.Angle; +import frc.robot.subsystems.lemon_hunter.LemonHunterConstants; + +public class HunterState { + + private static HunterState single_instance = null; + private Transform3d robotToHunterTransform = LemonHunterConstants.lemonHunterBaseTransform; + + private HunterState() {} + + public static synchronized HunterState getInstance() { + if (single_instance == null) single_instance = new HunterState(); + return single_instance; + } + + public Transform3d adjustIntakeExtenstion(Angle rotations){ + return robotToHunterTransform.plus(new Transform3d( + LemonHunterConstants.movementPerIntakeExtenstionRotation.times(rotations.in(Rotations)), + new Rotation3d(0, 0, 0)) + ); + } + + public Transform3d getRobotToHunterTransform(){ + return robotToHunterTransform; + } +} diff --git a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java index 6ac049d..99e35f5 100644 --- a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java @@ -17,4 +17,7 @@ private LemonHunterConstants() {} public static final Transform3d lemonHunterBaseTransform = new Transform3d( new Translation3d(0.3429, 0, 0.692), new Rotation3d(0, Math.toRadians(30), 0)); + + //Adjustment transform (TODO: Get Estimate) + public static final Translation3d movementPerIntakeExtenstionRotation = new Translation3d(0, 0, 0); } diff --git a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java index f494c86..5207f9e 100644 --- a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java @@ -1,16 +1,43 @@ package frc.robot.subsystems.lemon_hunter; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.ArrayList; +import java.util.List; + +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Importance; public class LemonHunterSubsystem extends SubsystemBase{ public LemonHunterSubsystem(){} + public ArrayList lemonList = new ArrayList(); + + @Logged(name = "Lemon Hunter/Results This Cycle", importance = Importance.CRITICAL) + private int resultsThisCycle = 0; + + @Logged(name = "Lemon Hunter/Lemons Found This Cycle") + private int lemonsFoundThisCycle = 0; + @Override public void periodic(){ - var lemonHunterResults = LemonHunterConstants.lemonHunterCamera.getAllUnreadResults(); + + lemonList.clear(); + List lemonHunterResults = LemonHunterConstants.lemonHunterCamera.getAllUnreadResults(); + resultsThisCycle = lemonHunterResults.size(); for(var result : lemonHunterResults){ - + for(var target : result.targets){ + + } } + lemonsFoundThisCycle = lemonList.size(); + } + + public void locateLemon(PhotonTrackedTarget target){ + } } From 5a3bf27b2478762e6091f5b9153ba95de75e97e4 Mon Sep 17 00:00:00 2001 From: Brandon Date: Sat, 4 Apr 2026 15:26:12 -0400 Subject: [PATCH 3/5] earlyKNN --- .../robot/subsystems/vision/LemonHunt.java | 84 +++++++++++++++++++ .../subsystems/vision/VisionConstants.java | 3 + 2 files changed, 87 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/vision/LemonHunt.java diff --git a/src/main/java/frc/robot/subsystems/vision/LemonHunt.java b/src/main/java/frc/robot/subsystems/vision/LemonHunt.java new file mode 100644 index 0000000..7b06c3d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/LemonHunt.java @@ -0,0 +1,84 @@ +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Pose3d; + +public class LemonHunt { + PhotonPipelineResult result = photonCamera_AI.getLatestResult(); + List inferences = result.getTargets(); + @Logged (name = "AI inferences Positions") + List pos = new ArrayList<>(); // idk if i should change to poe3d or translation + + @Logged(name = "nearest Targets") + List nearest_lemons = new ArrayList(); + + @Logged (name = "AI inferences") + List infer = photonCamera_AI.getAllUnreadResults(); + + private boolean hasTargets = result.hasTargets(); + public class LemonHunt(){ + + + } + + + + public void nearest_kluster(){ + //Short break to learn about this + } + + public Translation3d nearest(){ + double minDist = Double.MAX_VALUE; + for(PhotonTrackedTarget lemons: pos){ + Translation3d xyz = target.getBestCameraToTarget().getTranslation(); + double dist = xyz.getNorm(); + + if(dist < minDist){ + minDist = dist; + nearest = xyz; + } + + } + return nearest; + } + public List getNearestLemonBasket(){ + int lemon_count = 0; + for(Pose3d lemons: pos){ + if((lemons.getX < (nearest.getX + 1)) && (lemons.getZ < (nearest.getZ + 2))){ + int ID = lemons.getDetectedObjectClassID(); + nearest_lemons.add(ID, (lemons)); + lemon_count += 1; + } + + } + return nearest_lemons; + + } + public Pose3d getLemonDistances(Pose2d robotPose, PhotonTrackedTarget target, double cameraHeight, double objectHeight, double cameraPitch){ + double targetPitch = Math.toRadians(target.getPitch()); + double targetYaw = Math.toRadians(target.getYaw()); + + double distance = PhotonUtils.calculateDistanceToTarget( + cameraHeight, + objectHeight, + cameraPitch, + targetPitch + ); + double dx = distance * Math.cos(targetYaw); + double dy = distance * Math.sin(targetYaw); + + return new Pose3d( + robotPose.getX() + dx, + robotPose.getY() + dy, + objectHeight, + new Rotation3d(0, 0, targetYaw) + ); + + } + + } + + + diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 6eb0399..e1cd4b5 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -24,6 +24,9 @@ public class VisionConstants { // Right camera public static final String photonCameraName_Right = "RIGHT-CAMERA"; public static final PhotonCamera photonCamera_Right = new PhotonCamera(photonCameraName_Right); + //AI camera + public static final String photonCameraName_AI = "LemonHunter"; + public static final PhotonCamera photonCamera_AI = new PhotonCamera(photonCamera_AI); // TODO: TUNE public static final double XY_STD_DEV_COEFFICIENT = 0.01; From 6d6a695a206cec9899559297722c262e61849ddc Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sun, 5 Apr 2026 16:34:43 -0400 Subject: [PATCH 4/5] Constants added along with some revised code --- .../frc/robot/statemachines/HunterState.java | 19 ---------- .../lemon_hunter/LemonHunterConstants.java | 15 ++++---- .../lemon_hunter/LemonHunterSubsystem.java | 37 ++++++++++++++++++- .../subsystems/vision/VisionConstants.java | 3 -- 4 files changed, 43 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/statemachines/HunterState.java b/src/main/java/frc/robot/statemachines/HunterState.java index 531e1b6..e17bfa9 100644 --- a/src/main/java/frc/robot/statemachines/HunterState.java +++ b/src/main/java/frc/robot/statemachines/HunterState.java @@ -1,16 +1,8 @@ package frc.robot.statemachines; -import static edu.wpi.first.units.Units.Rotations; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.units.measure.Angle; -import frc.robot.subsystems.lemon_hunter.LemonHunterConstants; - public class HunterState { private static HunterState single_instance = null; - private Transform3d robotToHunterTransform = LemonHunterConstants.lemonHunterBaseTransform; private HunterState() {} @@ -18,15 +10,4 @@ public static synchronized HunterState getInstance() { if (single_instance == null) single_instance = new HunterState(); return single_instance; } - - public Transform3d adjustIntakeExtenstion(Angle rotations){ - return robotToHunterTransform.plus(new Transform3d( - LemonHunterConstants.movementPerIntakeExtenstionRotation.times(rotations.in(Rotations)), - new Rotation3d(0, 0, 0)) - ); - } - - public Transform3d getRobotToHunterTransform(){ - return robotToHunterTransform; - } } diff --git a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java index 99e35f5..185d45f 100644 --- a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java @@ -1,9 +1,14 @@ package frc.robot.subsystems.lemon_hunter; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; + import org.photonvision.PhotonCamera; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; public class LemonHunterConstants { @@ -13,11 +18,7 @@ private LemonHunterConstants() {} public static final String lemonHunterCameraName = "LEMON-HUNTER"; public static final PhotonCamera lemonHunterCamera = new PhotonCamera(lemonHunterCameraName); - //Base Transform (when intake is pulled in), actual transform updating as intake moves - public static final Transform3d lemonHunterBaseTransform = new Transform3d( - new Translation3d(0.3429, 0, 0.692), - new Rotation3d(0, Math.toRadians(30), 0)); - - //Adjustment transform (TODO: Get Estimate) - public static final Translation3d movementPerIntakeExtenstionRotation = new Translation3d(0, 0, 0); + public static final Distance HUNTER_OFFSET = Meters.of(0.61); + public static final Distance HUNTER_HEIGHT = Meters.of(0.6223); + public static final Angle HUNTER_PITCH = Degrees.of(-30); } diff --git a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java index 5207f9e..2cdc4f8 100644 --- a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java @@ -1,13 +1,23 @@ package frc.robot.subsystems.lemon_hunter; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.statemachines.DriveState; +import frc.robot.subsystems.vision.VisionConstants; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; + import java.util.ArrayList; import java.util.List; +import org.photonvision.PhotonUtils; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; @@ -17,6 +27,8 @@ public LemonHunterSubsystem(){} public ArrayList lemonList = new ArrayList(); + public DriveState driveState = DriveState.getInstance(); + @Logged(name = "Lemon Hunter/Results This Cycle", importance = Importance.CRITICAL) private int resultsThisCycle = 0; @@ -29,9 +41,30 @@ public void periodic(){ lemonList.clear(); List lemonHunterResults = LemonHunterConstants.lemonHunterCamera.getAllUnreadResults(); resultsThisCycle = lemonHunterResults.size(); - for(var result : lemonHunterResults){ - for(var target : result.targets){ + if(!lemonHunterResults.isEmpty()){ + //choose the latest result + PhotonPipelineResult latestResult = lemonHunterResults.get(lemonHunterResults.size()-1); + + for(var target : latestResult.targets){ + //uses the heights and pitches of the camera and target to calculate magnitude + double cameraToTargetDistance = PhotonUtils.calculateDistanceToTargetMeters( + LemonHunterConstants.HUNTER_HEIGHT.in(Meters), 0, LemonHunterConstants.HUNTER_PITCH.in(Radians), Math.toRadians(target.pitch)); + + //uses the pitch of the target and the magnitude to calculate the translation vector + Translation2d cameraToTarget2dTransform = PhotonUtils.estimateCameraToTargetTranslation(cameraToTargetDistance, new Rotation2d(Math.toRadians(target.yaw))); + + //adds the camera's offset from the robot's center to the x-coordinate fo the translation vector + cameraToTarget2dTransform = cameraToTarget2dTransform.plus(new Translation2d(LemonHunterConstants.HUNTER_OFFSET.in(Meters), 0)); + + //current robot pose + Pose2d currentRobotPose = driveState.getCurrentDriveStats().Pose; + + //calculates the position of the lemon using the robot's position + Pose2d lemonPose = new Pose2d(currentRobotPose.getX(), currentRobotPose.getY(), new Rotation2d()) + .plus(new Transform2d(cameraToTarget2dTransform.rotateBy(currentRobotPose.getRotation()), new Rotation2d())); + + lemonList.add(lemonPose); } } lemonsFoundThisCycle = lemonList.size(); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index e1cd4b5..6eb0399 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -24,9 +24,6 @@ public class VisionConstants { // Right camera public static final String photonCameraName_Right = "RIGHT-CAMERA"; public static final PhotonCamera photonCamera_Right = new PhotonCamera(photonCameraName_Right); - //AI camera - public static final String photonCameraName_AI = "LemonHunter"; - public static final PhotonCamera photonCamera_AI = new PhotonCamera(photonCamera_AI); // TODO: TUNE public static final double XY_STD_DEV_COEFFICIENT = 0.01; From 1ce3d96d26b70a0b0a1613e619f6fb285cfa15ae Mon Sep 17 00:00:00 2001 From: Brandon Date: Mon, 6 Apr 2026 20:11:38 -0400 Subject: [PATCH 5/5] LemonHunt --- .../lemon_hunter/LemonHunterSubsystem.java | 237 +++++++++++++++--- .../robot/subsystems/vision/LemonHunt.java | 84 ------- 2 files changed, 201 insertions(+), 120 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/vision/LemonHunt.java diff --git a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java index 2cdc4f8..4bcad46 100644 --- a/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java @@ -2,75 +2,240 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.statemachines.DriveState; -import frc.robot.subsystems.vision.VisionConstants; - import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; - import java.util.ArrayList; import java.util.List; - import org.photonvision.PhotonUtils; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; - 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.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; -public class LemonHunterSubsystem extends SubsystemBase{ - - public LemonHunterSubsystem(){} +public class LemonHunterSubsystem extends SubsystemBase { + + private static final double LEMON_DIAMETER_M = 0.15; + + private static final double CAMERA_FOCAL_LENGTH_PX = 600.0; + + private static final double CLUSTER_RADIUS_M = 0.5; + + private static final double OVERLAP_THRESHOLD_M = LEMON_DIAMETER_M; - public ArrayList lemonList = new ArrayList(); + public List lemonList = new ArrayList<>(); + + + public List bestCluster = new ArrayList<>(); + + + public List overlappingPairs = new ArrayList<>(); public DriveState driveState = DriveState.getInstance(); + private List latestTargets = new ArrayList<>(); + @Logged(name = "Lemon Hunter/Results This Cycle", importance = Importance.CRITICAL) private int resultsThisCycle = 0; - + @Logged(name = "Lemon Hunter/Lemons Found This Cycle") private int lemonsFoundThisCycle = 0; - @Override - public void periodic(){ + @Logged(name = "Lemon Hunter/Best Cluster Size") + private int bestClusterSize = 0; + + @Logged(name = "Lemon Hunter/Overlapping Pairs") + private int overlappingPairCount = 0; + + public LemonHunterSubsystem() {} + @Override + public void periodic() { lemonList.clear(); - List lemonHunterResults = LemonHunterConstants.lemonHunterCamera.getAllUnreadResults(); - resultsThisCycle = lemonHunterResults.size(); + bestCluster.clear(); + overlappingPairs.clear(); + latestTargets.clear(); + + List results = + LemonHunterConstants.lemonHunterCamera.getAllUnreadResults(); + resultsThisCycle = results.size(); + + if (results.isEmpty()) { + lemonsFoundThisCycle = 0; + bestClusterSize = 0; + return; + } - if(!lemonHunterResults.isEmpty()){ - //choose the latest result - PhotonPipelineResult latestResult = lemonHunterResults.get(lemonHunterResults.size()-1); + PhotonPipelineResult latestResult = results.get(results.size() - 1); + latestTargets.addAll(latestResult.targets); - for(var target : latestResult.targets){ - //uses the heights and pitches of the camera and target to calculate magnitude - double cameraToTargetDistance = PhotonUtils.calculateDistanceToTargetMeters( - LemonHunterConstants.HUNTER_HEIGHT.in(Meters), 0, LemonHunterConstants.HUNTER_PITCH.in(Radians), Math.toRadians(target.pitch)); - - //uses the pitch of the target and the magnitude to calculate the translation vector - Translation2d cameraToTarget2dTransform = PhotonUtils.estimateCameraToTargetTranslation(cameraToTargetDistance, new Rotation2d(Math.toRadians(target.yaw))); + Pose2d robotPose = driveState.getCurrentDriveStats().Pose; - //adds the camera's offset from the robot's center to the x-coordinate fo the translation vector - cameraToTarget2dTransform = cameraToTarget2dTransform.plus(new Translation2d(LemonHunterConstants.HUNTER_OFFSET.in(Meters), 0)); + for (var target : latestTargets) { + Pose3d lemonPose = estimateLemon3dPose(robotPose, target); + lemonList.add(lemonPose); + } - //current robot pose - Pose2d currentRobotPose = driveState.getCurrentDriveStats().Pose; - - //calculates the position of the lemon using the robot's position - Pose2d lemonPose = new Pose2d(currentRobotPose.getX(), currentRobotPose.getY(), new Rotation2d()) - .plus(new Transform2d(cameraToTarget2dTransform.rotateBy(currentRobotPose.getRotation()), new Rotation2d())); + lemonsFoundThisCycle = lemonList.size(); + + bestCluster = findLargestCluster(lemonList, CLUSTER_RADIUS_M); + bestClusterSize = bestCluster.size(); + + + overlappingPairs = detectOverlaps(bestCluster, OVERLAP_THRESHOLD_M); + overlappingPairCount = overlappingPairs.size(); + } + + public Pose3d estimateLemon3dPose(Pose2d robotPose, PhotonTrackedTarget target) { + double targetYaw = Math.toRadians(target.getYaw()); + double targetPitch = Math.toRadians(target.getPitch()); + double depthMeters; + + var detectedCorners = target.getDetectedCorners(); + if (detectedCorners != null && detectedCorners.size() >= 2) { + double minX = detectedCorners.stream().mapToDouble(c -> c.x).min().orElse(0); + double maxX = detectedCorners.stream().mapToDouble(c -> c.x).max().orElse(1); + double pixelWidth = maxX - minX; + + if (pixelWidth > 0) { - lemonList.add(lemonPose); + depthMeters = (LEMON_DIAMETER_M * CAMERA_FOCAL_LENGTH_PX) / pixelWidth; + } else { + depthMeters = pitchBasedDistance(target); } + } else { + depthMeters = pitchBasedDistance(target); } - lemonsFoundThisCycle = lemonList.size(); + + + double dx_cam = depthMeters * Math.cos(targetPitch) * Math.cos(targetYaw); + double dy_cam = depthMeters * Math.cos(targetPitch) * Math.sin(targetYaw); + double dz_cam = depthMeters * Math.sin(targetPitch); + + dx_cam += LemonHunterConstants.HUNTER_OFFSET.in(Meters); + + double heading = robotPose.getRotation().getRadians(); + double dx_field = dx_cam * Math.cos(heading) - dy_cam * Math.sin(heading); + double dy_field = dx_cam * Math.sin(heading) + dy_cam * Math.cos(heading); + double dz_field = dz_cam; + + return new Pose3d( + robotPose.getX() + dx_field, + robotPose.getY() + dy_field, + dz_field, + new Rotation3d(0, -targetPitch, targetYaw)); } - public void locateLemon(PhotonTrackedTarget target){ + + private double pitchBasedDistance(PhotonTrackedTarget target) { + return PhotonUtils.calculateDistanceToTargetMeters( + LemonHunterConstants.HUNTER_HEIGHT.in(Meters), + 0, + LemonHunterConstants.HUNTER_PITCH.in(Radians), + Math.toRadians(target.getPitch())); + } + + + public List findLargestCluster(List lemons, double radiusM) { + if (lemons.isEmpty()) return new ArrayList<>(); + + int n = lemons.size(); + int[] clusterID = new int[n]; + for (int i = 0; i < n; i++) clusterID[i] = i; + + for (int i = 0; i < n; i++) { + for (int j = i + 1; j < n; j++) { + if (distance3d(lemons.get(i), lemons.get(j)) <= radiusM) { + + int oldID = clusterID[j]; + int newID = clusterID[i]; + for (int k = 0; k < n; k++) { + if (clusterID[k] == oldID) clusterID[k] = newID; + } + } + } + } + + + int bestID = -1; + int bestCount = 0; + for (int i = 0; i < n; i++) { + int count = 0; + for (int j = 0; j < n; j++) { + if (clusterID[j] == clusterID[i]) count++; + } + if (count > bestCount) { + bestCount = count; + bestID = clusterID[i]; + } + } + + // Collect only lemons in the largest cluster + List result = new ArrayList<>(); + for (int i = 0; i < n; i++) { + if (clusterID[i] == bestID) result.add(lemons.get(i)); + } + return result; + } + + + public List detectOverlaps(List lemons, double thresholdM) { + List pairs = new ArrayList<>(); + for (int i = 0; i < lemons.size(); i++) { + for (int j = i + 1; j < lemons.size(); j++) { + if (distance3d(lemons.get(i), lemons.get(j)) <= thresholdM) { + pairs.add(new int[]{i, j}); + } + } + } + return pairs; + } + + public Pose3d getNearestLemonInCluster(Pose2d robotPose) { + if (bestCluster.isEmpty()) return null; + + Pose3d nearest = null; + double minDist = Double.MAX_VALUE; + + for (Pose3d lemon : bestCluster) { + double dist = Math.hypot( + lemon.getX() - robotPose.getX(), + lemon.getY() - robotPose.getY()); + if (dist < minDist) { + minDist = dist; + nearest = lemon; + } + } + return nearest; + } + + + public Pose2d getClusterCentroid(Pose2d robotPose) { + if (bestCluster.isEmpty()) return null; + + double sumX = 0, sumY = 0; + for (Pose3d lemon : bestCluster) { + sumX += lemon.getX(); + sumY += lemon.getY(); + } + double cx = sumX / bestCluster.size(); + double cy = sumY / bestCluster.size(); + + double angle = Math.atan2(cy - robotPose.getY(), cx - robotPose.getX()); + return new Pose2d(cx, cy, new Rotation2d(angle)); + } + + private double distance3d(Pose3d a, Pose3d b) { + double dx = a.getX() - b.getX(); + double dy = a.getY() - b.getY(); + double dz = a.getZ() - b.getZ(); + return Math.sqrt(dx * dx + dy * dy + dz * dz); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/LemonHunt.java b/src/main/java/frc/robot/subsystems/vision/LemonHunt.java deleted file mode 100644 index 7b06c3d..0000000 --- a/src/main/java/frc/robot/subsystems/vision/LemonHunt.java +++ /dev/null @@ -1,84 +0,0 @@ -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Pose3d; - -public class LemonHunt { - PhotonPipelineResult result = photonCamera_AI.getLatestResult(); - List inferences = result.getTargets(); - @Logged (name = "AI inferences Positions") - List pos = new ArrayList<>(); // idk if i should change to poe3d or translation - - @Logged(name = "nearest Targets") - List nearest_lemons = new ArrayList(); - - @Logged (name = "AI inferences") - List infer = photonCamera_AI.getAllUnreadResults(); - - private boolean hasTargets = result.hasTargets(); - public class LemonHunt(){ - - - } - - - - public void nearest_kluster(){ - //Short break to learn about this - } - - public Translation3d nearest(){ - double minDist = Double.MAX_VALUE; - for(PhotonTrackedTarget lemons: pos){ - Translation3d xyz = target.getBestCameraToTarget().getTranslation(); - double dist = xyz.getNorm(); - - if(dist < minDist){ - minDist = dist; - nearest = xyz; - } - - } - return nearest; - } - public List getNearestLemonBasket(){ - int lemon_count = 0; - for(Pose3d lemons: pos){ - if((lemons.getX < (nearest.getX + 1)) && (lemons.getZ < (nearest.getZ + 2))){ - int ID = lemons.getDetectedObjectClassID(); - nearest_lemons.add(ID, (lemons)); - lemon_count += 1; - } - - } - return nearest_lemons; - - } - public Pose3d getLemonDistances(Pose2d robotPose, PhotonTrackedTarget target, double cameraHeight, double objectHeight, double cameraPitch){ - double targetPitch = Math.toRadians(target.getPitch()); - double targetYaw = Math.toRadians(target.getYaw()); - - double distance = PhotonUtils.calculateDistanceToTarget( - cameraHeight, - objectHeight, - cameraPitch, - targetPitch - ); - double dx = distance * Math.cos(targetYaw); - double dy = distance * Math.sin(targetYaw); - - return new Pose3d( - robotPose.getX() + dx, - robotPose.getY() + dy, - objectHeight, - new Rotation3d(0, 0, targetYaw) - ); - - } - - } - - -