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..e17bfa9 --- /dev/null +++ b/src/main/java/frc/robot/statemachines/HunterState.java @@ -0,0 +1,13 @@ +package frc.robot.statemachines; + +public class HunterState { + + private static HunterState single_instance = null; + + private HunterState() {} + + public static synchronized HunterState getInstance() { + if (single_instance == null) single_instance = new HunterState(); + return single_instance; + } +} 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..185d45f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterConstants.java @@ -0,0 +1,24 @@ +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 { + + private LemonHunterConstants() {} + + //Camera Constants + public static final String lemonHunterCameraName = "LEMON-HUNTER"; + public static final PhotonCamera lemonHunterCamera = new PhotonCamera(lemonHunterCameraName); + + 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/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..4bcad46 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/lemon_hunter/LemonHunterSubsystem.java @@ -0,0 +1,241 @@ +package frc.robot.subsystems.lemon_hunter; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.statemachines.DriveState; +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 { + + 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 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; + + @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(); + bestCluster.clear(); + overlappingPairs.clear(); + latestTargets.clear(); + + List results = + LemonHunterConstants.lemonHunterCamera.getAllUnreadResults(); + resultsThisCycle = results.size(); + + if (results.isEmpty()) { + lemonsFoundThisCycle = 0; + bestClusterSize = 0; + return; + } + + PhotonPipelineResult latestResult = results.get(results.size() - 1); + latestTargets.addAll(latestResult.targets); + + Pose2d robotPose = driveState.getCurrentDriveStats().Pose; + + for (var target : latestTargets) { + Pose3d lemonPose = estimateLemon3dPose(robotPose, target); + lemonList.add(lemonPose); + } + + 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) { + + depthMeters = (LEMON_DIAMETER_M * CAMERA_FOCAL_LENGTH_PX) / pixelWidth; + } else { + depthMeters = pitchBasedDistance(target); + } + } else { + depthMeters = pitchBasedDistance(target); + } + + + 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)); + } + + + 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