diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 40350b2b88..7e23a43cab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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 diff --git a/src/main/java/frc/robot/vision/DetectedObjectType.java b/src/main/java/frc/robot/vision/DetectedObjectType.java index d15b85e7fe..d9330c65c2 100644 --- a/src/main/java/frc/robot/vision/DetectedObjectType.java +++ b/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; 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 ab97024894..d409d66f68 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/Limelight.java +++ b/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,10 +22,13 @@ 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; @@ -32,6 +36,10 @@ 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; 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(); 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..7b71dbaa00 100644 --- a/src/main/java/frc/robot/vision/cameras/limelight/ObjectDetectionMath.java +++ b/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, diff --git a/src/main/python/objectdetection/SnapscriptDetectionCode.py b/src/main/python/objectdetection/SnapscriptDetectionCode.py new file mode 100644 index 0000000000..eff51f31ca --- /dev/null +++ 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