diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c64a5aae..8a5b66e6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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)); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index d430ec25..9aeaf8a2 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/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; } diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 7cfbe916..7ecad76f 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/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 system; private final Supplier pose2dSupplier; + Matrix algaeConfidences = new Matrix(Nat.N12(), Nat.N1(), new double[12]); ArrayList currentAlgaePosition = new ArrayList<>(); + Matrix coralConfidences = new Matrix(Nat.N6(), Nat.N6(), new double[36]); ArrayList currentCoralPositions = new ArrayList<>(); public Vision(VisionIO io, Supplier 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); } } } diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 2eba35da..209630c8 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/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 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 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 cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix 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 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; } } diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index f59effb6..8141e507 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/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); } }