Object detection -> master

src/main/java/frc/robot/Robot.java

@@ -176,7 +176,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)
 		);
 		this.limelightRight = new Limelight(
 			"limelight-right",
@@ -185,16 +187,20 @@ 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)
 		);
 		this.limelightLeft = new Limelight(
-			"limelight-left",
+			"limelight-two",
 			"Vision",
 			new Pose3d(
 				new Translation3d(-0.125, -0.37, 0.481),
 				new Rotation3d(Math.toRadians(-179.25), Math.toRadians(20.05), Math.toRadians(90.35))
 			),
-			LimelightPipeline.APRIL_TAG
+			LimelightPipeline.APRIL_TAG,
+			Rotation2d.fromDegrees(62.5),
+			Rotation2d.fromDegrees(48.9)
 		);
 
 		this.limelights = List.of(limelightFront, limelightRight, limelightLeft);
@@ -322,6 +328,18 @@ public void periodic() {
 		getLimelights().forEach(Limelight::updateHardwareInputs);
 		getLimelights().forEach(Limelight::updateMT1);
 		getLimelights().forEach(limelight -> limelight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision));
+		limelightFront.updateHardwareInputs();
+		limelightRight.updateHardwareInputs();
+		limelightLeft.updateHardwareInputs();
+
+		limelightFront.updateMT1();
+		limelightRight.updateMT1();
+		limelightLeft.updateMT1();
+		limelightLeft.updateHeatMapObjectDetection();
+
+		limelightFront.getIndependentRobotPose().ifPresent(poseEstimator::updateVision);
+		limelightRight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision);
+		limelightLeft.getIndependentRobotPose().ifPresent(poseEstimator::updateVision);
 
 		poseEstimator.log();
 		ShootingCalculations

src/main/java/frc/robot/vision/DetectedObjectType.java

@@ -3,7 +3,8 @@
 import java.util.Optional;
 
 public enum DetectedObjectType {
-	;
+
+	FUEL("Fuel", 0.075);
 
 	private final String name;
 	private final double centerHeightFromFloorMeters;

src/main/java/frc/robot/vision/cameras/limelight/Limelight.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.*;
 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;
@@ -21,17 +22,24 @@
 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 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;
 	private final Pose3d robotRelativeCameraPose;
 
 	private final ArrayList<DetectedObjectObservation> neuralDetections;
 	private final ArrayList<DetectedObjectObservation> colorDetections;
+	private final ArrayList<DetectedObjectObservation> weirdObjectThing;
+
+	private final Rotation2d horizontalFOV;
+	private final Rotation2d verticalFOV;
 
 	private final LimelightInputsSet inputs;
 
@@ -50,7 +58,14 @@ 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;
 
@@ -59,6 +74,10 @@ 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;
 
 		this.mt1PoseObservation = new RobotPoseObservation();
 		this.mt2PoseObservation = new RobotPoseObservation();
@@ -109,6 +128,34 @@ 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() * THREE_FOURTHS_OBJECT_FACTOR,
+			pitchOffset
+		);
+
+		double objectRelativeToCameraY = getCameraRelativeObjectY(
+			robotRelativeCameraPose,
+			DetectedObjectType.FUEL.getCenterHeightFromFloorMeters() * THREE_FOURTHS_OBJECT_FACTOR,
+			yawOffset,
+			objectRelativeToCameraX
+		);
+
+		Translation2d objectRelativeToCamera = new Translation2d(objectRelativeToCameraX, objectRelativeToCameraY);
+		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);
+	}
+
 	public void updateColorDetection() {
 		if (pipeline.isColorDetecting()) {
 			colorDetections.clear();

src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java

@@ -37,13 +37,13 @@ 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,

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