Changed piece detection implementation to be confidence-based

src/main/java/frc/robot/RobotContainer.java

@@ -474,7 +474,6 @@ public RobotVisualizer getRobotVisualizer() {
   }
 
   public void putShuffleboardCommands() {
-
     if (Constants.CORAL_DEBUG) {
       SmartDashboard.putData(
           "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED));

src/main/java/frc/robot/constants/GameConstants.java

@@ -236,6 +236,9 @@ public enum Mode {
       new Transform3d(
           0.3429, -0.0635, 0, new Rotation3d(0.0, (-47 * Math.PI) / 180, 0.0)); // z = 0.720725
   public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later
-  public static final double MINIMUM_PIECE_DETECTION_DOT = 0;
+  public static final double MINIMUM_PIECE_DETECTION_DOT = 0.5;
+  public static final double PIECE_DETECTION_PROBABILITY_SCALAR = 0.195571; // TODO: change later
+  public static final double MINUMUM_PIECE_DETECTION_CONFIRMED_DOT = 0.8;
+  public static final double DECAY_CONSTANT = 1.0002; // TODO: change later
   public static final boolean ENABLE_FANCY_LIMELIGHT_MATH = false;
 }

src/main/java/frc/robot/subsystems/limelight/Vision.java

@@ -4,7 +4,12 @@
 
 package frc.robot.subsystems.limelight;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N12;
+import edu.wpi.first.math.numbers.N6;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AlgaePositions;
 import frc.robot.constants.BranchPositions;
@@ -18,7 +23,9 @@
 public class Vision extends SubsystemBase {
   LoggableSystem<VisionIO, VisionInputs> system;
   private final Supplier<Pose2d> pose2dSupplier;
+  Matrix<N12, N1> algaeConfidences = new Matrix<N12, N1>(Nat.N12(), Nat.N1(), new double[12]);
   ArrayList<AlgaePositions> currentAlgaePosition = new ArrayList<>();
+  Matrix<N6, N6> coralConfidences = new Matrix<N6, N6>(Nat.N6(), Nat.N6(), new double[36]);
   ArrayList<BranchPositions> currentCoralPositions = new ArrayList<>();
 
   public Vision(VisionIO io, Supplier<Pose2d> pose2dSupplier) {
@@ -59,6 +66,8 @@ public void periodic() {
       locateGamePieces();
       Logger.recordOutput("coralPoses", getAllBranchPosition());
       Logger.recordOutput("algaePoses", getAllAlgaePosition());
+      algaeConfidences.elementPower(Constants.DECAY_CONSTANT);
+      coralConfidences.elementPower(Constants.DECAY_CONSTANT);
     }
   }
 
@@ -71,19 +80,36 @@ private void locateGamePieces() {
     }
     for (int i = 0; i < detectionLength; i++) {
       String className = system.getInputs().className[i];
-      if (className.equalsIgnoreCase("coral")) {
-        BranchPositions coralBranch =
+      if (className.equalsIgnoreCase("algae")) {
+        double[] returnArray =
             GamePieceLocate.findCoralBranch(
                 pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]);
-        if (coralBranch != null) {
-          currentCoralPositions.add(coralBranch);
+        AlgaePositions algaePos = AlgaePositions.values()[(int) returnArray[0]];
+        algaeConfidences.set(
+            (int) returnArray[0],
+            1,
+            (algaeConfidences.get((int) (returnArray[0]), 1)
+                + Math.pow(returnArray[2], 3)
+                    * Math.pow(returnArray[1], 2)
+                    * Constants.PIECE_DETECTION_PROBABILITY_SCALAR));
+        if (algaeConfidences.get((int) (returnArray[0] / 6), (int) (returnArray[0] % 6))
+            > Constants.MINUMUM_PIECE_DETECTION_CONFIRMED_DOT) {
+          currentAlgaePosition.add(algaePos);
         }
-      } else if (className.equalsIgnoreCase("algae")) {
-        AlgaePositions algaePos =
-            GamePieceLocate.findAlgaePos(
+      } else if (className.equalsIgnoreCase("coral")) {
+        double[] returnArray =
+            GamePieceLocate.findCoralBranch(
                 pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]);
-        if (algaePos != null) {
-          currentAlgaePosition.add(algaePos);
+        BranchPositions coralBranch = BranchPositions.values()[(int) returnArray[0]];
+        coralConfidences.set(
+            ((int) returnArray[0]) / 6,
+            ((int) returnArray[1]) % 6,
+            Math.pow(returnArray[2], 3)
+                * Math.pow(returnArray[1], 2)
+                * Constants.PIECE_DETECTION_PROBABILITY_SCALAR);
+        if (coralConfidences.get((int) (returnArray[0] / 6), (int) (returnArray[0] % 6))
+            > Constants.MINUMUM_PIECE_DETECTION_CONFIRMED_DOT) {
+          currentCoralPositions.add(coralBranch);
         }
       }
     }

src/main/java/frc/robot/utils/GamePieceLocate.java

@@ -28,7 +28,7 @@ public class GamePieceLocate {
           .toArray(Vector[]::new);
 
   // piece pos is in DEGREES, not RD
-  public static BranchPositions findCoralBranch(
+  public static double[] findCoralBranch(
       Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) {
     final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT);
     final Vector<N3> cameraPosVec = cameraPos.getTranslation().toVector();
@@ -40,8 +40,10 @@ public static BranchPositions findCoralBranch(
                 Math.tan(Math.toRadians(piecePosTYDeg)))
             .unit();
     double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT;
-    BranchPositions closest = null;
     int n = CenterPositions.getClosest(robotPos);
+    int maxf = 0;
+    double secondDot = Constants.MINIMUM_PIECE_DETECTION_DOT;
+    double[] returnArray = new double[3];
     for (int i = 6 * n - 3; i < 6 * n + 9; i++) {
       int f = Math.floorMod(i, BRANCHES.length);
       Matrix<N3, N1> locationVec =
@@ -51,17 +53,22 @@ public static BranchPositions findCoralBranch(
               VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0))
                   .unit());
       if (dot > maxDot) {
+        secondDot = maxDot;
         maxDot = dot;
-        closest = BRANCHES[f];
+        maxf = f;
+      }
+      if (dot > secondDot && dot != maxDot) {
+        secondDot = dot;
       }
     }
-
-    return closest;
+    returnArray[0] = maxf;
+    returnArray[1] = maxDot - secondDot;
+    returnArray[2] = maxDot;
+    return returnArray;
   }
 
   // piece pos is in DEGREES, not RD
-  public static AlgaePositions findAlgaePos(
-      Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) {
+  public static double[] findAlgaePos(Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) {
     final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT);
     final Vector<N3> cameraPosVec = cameraPos.getTranslation().toVector();
     final Matrix<N3, N3> invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix();
@@ -74,6 +81,9 @@ public static AlgaePositions findAlgaePos(
     double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT;
     AlgaePositions closest = null;
     int n = CenterPositions.getClosest(robotPos);
+    int maxf = 0;
+    double secondDot = Constants.MINIMUM_PIECE_DETECTION_DOT;
+    double[] returnArray = new double[3];
     for (int i = 2 * n - 2; i < 2 * n + 4; i++) {
       int f = Math.floorMod(i, ALGAES.length);
       Matrix<N3, N1> locationVec =
@@ -83,10 +93,16 @@ public static AlgaePositions findAlgaePos(
               VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0))
                   .unit());
       if (dot > maxDot) {
+        secondDot = maxDot;
         maxDot = dot;
-        closest = ALGAES[f];
+        maxf = f;
+      } else if (dot > secondDot && dot != maxDot) {
+        secondDot = dot;
       }
     }
-    return closest;
+    returnArray[0] = maxf;
+    returnArray[1] = maxDot - secondDot;
+    returnArray[2] = maxDot;
+    return returnArray;
   }
 }

src/test/java/frc/robot/utils/GamePieceLocateTest.java

@@ -16,25 +16,37 @@ void findAlgaePos() {
     double txDeg = 0; // degrees
     double dyDeg = -5; // degrees
     AlgaePositions algaePos =
-        GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg);
+        AlgaePositions.values()[
+            (int)
+                GamePieceLocate.findAlgaePos(
+                    new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]];
     assertEquals(AlgaePositions.Algae_AB_LOW, algaePos);
 
     txDeg = 0; // degrees
     dyDeg = 20; // degrees
     algaePos =
-        GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg);
+        AlgaePositions.values()[
+            (int)
+                GamePieceLocate.findAlgaePos(
+                    new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]];
     assertEquals(AlgaePositions.Algae_AB_HIGH, algaePos);
 
     txDeg = 10; // degrees
     dyDeg = 0; // degrees
     algaePos =
-        GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg);
+        AlgaePositions.values()[
+            (int)
+                GamePieceLocate.findAlgaePos(
+                    new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]];
     assertEquals(AlgaePositions.Algae_CD_LOW, algaePos);
 
     txDeg = -10; // degrees
     dyDeg = 0; // degrees
     algaePos =
-        GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg);
+        AlgaePositions.values()[
+            (int)
+                GamePieceLocate.findAlgaePos(
+                    new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]];
     assertEquals(AlgaePositions.Algae_KL_LOW, algaePos);
   }
 }