From 0336b4cd515ed5ca9ac3bb9448ed1e6ea647392c Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Fri, 21 Mar 2025 23:53:27 -0400 Subject: [PATCH 01/48] some small stuff --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../commands/FindCorrectBranchFromPos.java | 84 ++++++++++++ .../java/frc/robot/commands/UnitTester.java | 16 +++ .../frc/robot/constants/AlgaePositions.java | 52 ++++++++ .../frc/robot/constants/BranchPositions.java | 126 ++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 10 ++ 6 files changed, 294 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java create mode 100644 src/main/java/frc/robot/commands/UnitTester.java create mode 100644 src/main/java/frc/robot/constants/AlgaePositions.java create mode 100644 src/main/java/frc/robot/constants/BranchPositions.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 424ee467..51287d68 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.autochooser.FieldLocation; import frc.robot.autochooser.chooser.AutoChooser2025; import frc.robot.autochooser.event.RealAutoEventProvider; +import frc.robot.commands.UnitTester; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; import frc.robot.commands.climber.ClimbToLimit; @@ -78,6 +79,7 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; +import frc.robot.utils.shuffleboard.SmartShuffleboard; import frc.robot.utils.simulation.RobotVisualizer; import frc.robot.utils.simulation.SwerveSimulationUtils; import java.util.function.Consumer; @@ -250,7 +252,9 @@ private void configureBindings() { new SetElevatorTargetPosition(controller::getLeftY, elevatorSubsystem); elevatorSubsystem.setDefaultCommand(setElevatorTargetPosition); controller.b().onTrue(new ClimbToLimit(climber, Constants.CLIMBER_PHASE2_SPEED)); - controller.a().onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); + controller + .a() + .onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); // controller.a().onTrue(new DeployClimber(climber)); controller.x().onTrue(new ByeByeAllDone(byebyeTilt, byebyeRoller, elevatorSubsystem)); controller.y().onTrue(new RemoveAlgaeFromReef(byebyeTilt, byebyeRoller, elevatorSubsystem)); @@ -436,7 +440,7 @@ public RobotVisualizer getRobotVisualizer() { } public void putShuffleboardCommands() { - + SmartShuffleboard.putCommand("What Branch Unit Test", "Test 1", new UnitTester()); if (Constants.CORAL_DEBUG) { SmartDashboard.putData( "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java new file mode 100644 index 00000000..2ca339c7 --- /dev/null +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -0,0 +1,84 @@ +package frc.robot.commands; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N3; +import frc.robot.constants.AlgaePositions; +import frc.robot.constants.BranchPositions; +import frc.robot.constants.Constants; + +public class FindCorrectBranchFromPos { + public static final double POS_TO_FOV_CONVERSION_X = + Math.tan(Constants.LIMELIGHT_HALF_FOV.getX()) / Constants.LIMELIGHT_HALF_POS.getX(); + public static final double POS_TO_FOV_CONVERSION_Y = + Math.tan(Constants.LIMELIGHT_HALF_FOV.getY()) / Constants.LIMELIGHT_HALF_POS.getY(); + public static final double SPECIAL_X = + Constants.LIMELIGHT_HALF_POS.getX() / Math.tan(Constants.LIMELIGHT_HALF_FOV.getX()); + public static final double SPECIAL_Y = + Constants.LIMELIGHT_HALF_POS.getY() / Math.tan(Constants.LIMELIGHT_HALF_FOV.getY()); + public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; + private static final BranchPositions[] BRANCHES = BranchPositions.values(); + private static final AlgaePositions[] ALGAES = AlgaePositions.values(); + + public static BranchPositions UnitTest1() { + double x = -4.1902085050000 / 3.8337244850000 * SPECIAL_X + Constants.LIMELIGHT_HALF_POS.getX(); + double y = -0.706978325 / 3.8337244850000 * SPECIAL_Y + Constants.LIMELIGHT_HALF_POS.getY(); + return FindCoralBranch(new Pose2d(0, 0, new Rotation2d(0)), new Translation2d(x, y)); + } + + public static BranchPositions FindCoralBranch(Pose2d robotPos, Translation2d piecePos) { + final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); + final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); + final Translation2d pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); + final Vector pieceVec = + VecBuilder.fill(SPECIAL_Y, pieceTranslation.getX() * SPECIAL_RATIO, pieceTranslation.getY()) + .unit(); + double maxDot = -1.0; + BranchPositions closestBranch = null; + for (BranchPositions branch : BRANCHES) { + Vector branchVec = + branch + .getPosition() + .minus(cameraPos) + .getTranslation() + .rotateBy(invCameraRotation) + .toVector() + .unit(); + double dot = pieceVec.dot(branchVec); + if (dot > maxDot) { + maxDot = dot; + closestBranch = branch; + } + } + return closestBranch; + } + + public static AlgaePositions FindAlgae(Pose2d robotPos, Translation2d piecePos) { + final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); + final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); + final Translation2d pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); + + final Vector pieceVec = + VecBuilder.fill(SPECIAL_Y, pieceTranslation.getX() * SPECIAL_RATIO, pieceTranslation.getY()) + .unit(); + double maxDot = -1.0; + AlgaePositions closestAlgae = null; + for (AlgaePositions algae : ALGAES) { + Vector algaeVec = + algae + .getPosition() + .minus(cameraPos) + .getTranslation() + .rotateBy(invCameraRotation) + .toVector() + .unit(); + double dot = pieceVec.dot(algaeVec); + if (dot > maxDot) { + maxDot = dot; + closestAlgae = algae; + } + } + return closestAlgae; + } +} diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java new file mode 100644 index 00000000..15a090ce --- /dev/null +++ b/src/main/java/frc/robot/commands/UnitTester.java @@ -0,0 +1,16 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utils.shuffleboard.SmartShuffleboard; + +public class UnitTester extends Command { + @Override + public void execute() { + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/constants/AlgaePositions.java b/src/main/java/frc/robot/constants/AlgaePositions.java new file mode 100644 index 00000000..12d63fdf --- /dev/null +++ b/src/main/java/frc/robot/constants/AlgaePositions.java @@ -0,0 +1,52 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; + +public enum AlgaePositions { + Algae_AB_LOW( + new Pose3d( + 3.8337244850000, 4.0259000000000, 0.909311815, new Rotation3d(0, 0, 3.1415926535898))), + Algae_AB_HIGH( + new Pose3d( + 3.8337244850000, 4.0259000000000, 1.313978795, new Rotation3d(0, 0, 3.1415926535898))), + Algae_CD_LOW( + new Pose3d( + 4.1615306625000, 3.4581230455351, 0.909311815, new Rotation3d(0, 0, 4.1887902047864))), + Algae_CD_HIGH( + new Pose3d( + 4.1615306625000, 3.4581230455351, 1.313978795, new Rotation3d(0, 0, 4.1887902047864))), + Algae_EF_LOW( + new Pose3d( + 4.8171430175000, 3.4581230455351, 0.909311815, new Rotation3d(0, 0, 5.2359877559830))), + Algae_EF_HIGH( + new Pose3d( + 4.8171430175000, 3.4581230455351, 1.313978795, new Rotation3d(0, 0, 5.2359877559830))), + Algae_GH_LOW( + new Pose3d( + 5.1449491950000, 4.0259000000000, 0.909311815, new Rotation3d(0, 0, 6.2831853071796))), + Algae_GH_HIGH( + new Pose3d( + 5.1449491950000, 4.0259000000000, 1.313978795, new Rotation3d(0, 0, 6.2831853071796))), + Algae_IJ_LOW( + new Pose3d( + 4.8171430175000, 4.5936769544649, 0.909311815, new Rotation3d(0, 0, 7.3303828583762))), + Algae_IJ_HIGH( + new Pose3d( + 4.8171430175000, 4.5936769544649, 1.313978795, new Rotation3d(0, 0, 7.3303828583762))), + Algae_KL_LOW( + new Pose3d( + 4.1615306625000, 4.5936769544649, 0.909311815, new Rotation3d(0, 0, 8.3775804095728))), + Algae_KL_HIGH( + new Pose3d( + 4.1615306625000, 4.5936769544649, 1.313978795, new Rotation3d(0, 0, 8.3775804095728))); + private final Pose3d position; + + AlgaePositions(Pose3d position) { + this.position = position; + } + + public Pose3d getPosition() { + return position; + } +} diff --git a/src/main/java/frc/robot/constants/BranchPositions.java b/src/main/java/frc/robot/constants/BranchPositions.java new file mode 100644 index 00000000..ff961848 --- /dev/null +++ b/src/main/java/frc/robot/constants/BranchPositions.java @@ -0,0 +1,126 @@ +/** Technically not used now, mainly just for checking if the math is right */ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.*; + +// TODO: change all of these later to the correct values +public enum BranchPositions { + BRANCH_A_2( + new Pose3d( + 3.8337244850000, 4.1902085050000, 0.706978325, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_A_3( + new Pose3d( + 3.8337244850000, 4.1902085050000, 1.111645305, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_A_4( + new Pose3d( + 3.6896079100000, 4.1902085050000, 1.736182305, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_B_2( + new Pose3d( + 3.8337244850000, 3.8615914950000, 0.706978325, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_B_3( + new Pose3d( + 3.8337244850000, 3.8615914950000, 1.111645305, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_B_4( + new Pose3d( + 3.6896079100000, 3.8615914950000, 1.736182305, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_C_2( + new Pose3d( + 4.0192353231122, 3.5402772980351, 0.706978325, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_C_3( + new Pose3d( + 4.0192353231122, 3.5402772980351, 1.111645305, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_C_4( + new Pose3d( + 3.9471770356122, 3.4154686829787, 1.736182305, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_D_2( + new Pose3d( + 4.3038260018878, 3.3759687930351, 0.706978325, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_D_3( + new Pose3d( + 4.3038260018878, 3.3759687930351, 1.111645305, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_D_4( + new Pose3d( + 4.2317677143878, 3.2511601779787, 1.736182305, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_E_2( + new Pose3d( + 4.6748476781122, 3.3759687930351, 0.706978325, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_E_3( + new Pose3d( + 4.6748476781122, 3.3759687930351, 1.111645305, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_E_4( + new Pose3d( + 4.7469059656122, 3.2511601779787, 1.736182305, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_F_2( + new Pose3d( + 4.9594383568878, 3.5402772980351, 0.706978325, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_F_3( + new Pose3d( + 4.9594383568878, 3.5402772980351, 1.111645305, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_F_4( + new Pose3d( + 5.0314966443878, 3.4154686829787, 1.736182305, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_G_2( + new Pose3d( + 5.1449491950000, 3.8615914950000, 0.706978325, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_G_3( + new Pose3d( + 5.1449491950000, 3.8615914950000, 1.111645305, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_G_4( + new Pose3d( + 5.2890657700000, 3.8615914950000, 1.736182305, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_H_2( + new Pose3d( + 5.1449491950000, 4.1902085050000, 0.706978325, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_H_3( + new Pose3d( + 5.1449491950000, 4.1902085050000, 1.111645305, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_H_4( + new Pose3d( + 5.2890657700000, 4.1902085050000, 1.736182305, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_I_2( + new Pose3d( + 4.9594383568878, 4.5115227019649, 0.706978325, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_I_3( + new Pose3d( + 4.9594383568878, 4.5115227019649, 1.111645305, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_I_4( + new Pose3d( + 5.0314966443878, 4.6363313170214, 1.736182305, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_J_2( + new Pose3d( + 4.6748476781122, 4.6758312069649, 0.706978325, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_J_3( + new Pose3d( + 4.6748476781122, 4.6758312069649, 1.111645305, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_J_4( + new Pose3d( + 4.7469059656122, 4.8006398220214, 1.736182305, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_K_2( + new Pose3d( + 4.3038260018878, 4.6758312069649, 0.706978325, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_K_3( + new Pose3d( + 4.3038260018878, 4.6758312069649, 1.111645305, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_K_4( + new Pose3d( + 4.2317677143878, 4.8006398220214, 1.736182305, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_L_2( + new Pose3d( + 4.0192353231122, 4.5115227019649, 0.706978325, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_L_3( + new Pose3d( + 4.0192353231122, 4.5115227019649, 1.111645305, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_L_4( + new Pose3d( + 3.9471770356122, 4.6363313170214, 1.736182305, new Rotation3d(0, 0, 8.3775804095728))); + + private final Pose3d position; + + BranchPositions(Pose3d position) { + this.position = position; + } + + public Pose3d getPosition() { + return position; + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 09cfc094..c56ebce8 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,6 +1,9 @@ package frc.robot.constants; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.utils.SwerveModuleProfileV2; @@ -218,4 +221,11 @@ public enum Mode { public static final double ELEVATOR_MANUAL_DEADBAND = 0.2; public static final double ELEVATOR_MANUAL_MAX_SPEED_UP = -.3; public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; + + // Limelight + public static final Translation2d LIMELIGHT_HALF_POS = new Translation2d(240, 320).div(2); + public static final Transform3d CAMERA_TO_ROBOT = + new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later + public static final Translation2d LIMELIGHT_HALF_FOV = + new Translation2d(0.86742863824118179973, 1.04021623418862042785).div(2); } From 416b0409782038073df54a5f529c901d7edb4037 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 12:29:22 -0400 Subject: [PATCH 02/48] MORE linear algebra --- .../commands/FindCorrectBranchFromPos.java | 74 +++++++++---------- .../java/frc/robot/commands/UnitTester.java | 5 +- .../frc/robot/constants/GameConstants.java | 11 ++- 3 files changed, 43 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 2ca339c7..4ad6b4d8 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -3,82 +3,78 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N3; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; +import java.util.Arrays; public class FindCorrectBranchFromPos { - public static final double POS_TO_FOV_CONVERSION_X = - Math.tan(Constants.LIMELIGHT_HALF_FOV.getX()) / Constants.LIMELIGHT_HALF_POS.getX(); - public static final double POS_TO_FOV_CONVERSION_Y = - Math.tan(Constants.LIMELIGHT_HALF_FOV.getY()) / Constants.LIMELIGHT_HALF_POS.getY(); public static final double SPECIAL_X = - Constants.LIMELIGHT_HALF_POS.getX() / Math.tan(Constants.LIMELIGHT_HALF_FOV.getX()); + Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); public static final double SPECIAL_Y = - Constants.LIMELIGHT_HALF_POS.getY() / Math.tan(Constants.LIMELIGHT_HALF_FOV.getY()); + Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); + private static final Vector[] PRECOMPUTED_BRANCH_VECS = + Arrays.stream(BranchPositions.values()) + .map(branch -> branch.getPosition().getTranslation().toVector()) + .toArray(Vector[]::new); + private static final Vector[] PRECOMPUTED_ALGAE_VECS = + Arrays.stream(AlgaePositions.values()) + .map(algae -> algae.getPosition().getTranslation().toVector()) + .toArray(Vector[]::new); - public static BranchPositions UnitTest1() { - double x = -4.1902085050000 / 3.8337244850000 * SPECIAL_X + Constants.LIMELIGHT_HALF_POS.getX(); - double y = -0.706978325 / 3.8337244850000 * SPECIAL_Y + Constants.LIMELIGHT_HALF_POS.getY(); - return FindCoralBranch(new Pose2d(0, 0, new Rotation2d(0)), new Translation2d(x, y)); - } - - public static BranchPositions FindCoralBranch(Pose2d robotPos, Translation2d piecePos) { + public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Translation2d pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); + final Vector cameraPosVec = cameraPos.getTranslation().toVector(); + final Vector invCameraRotation = cameraPos.getRotation().unaryMinus().toVector(); + final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill(SPECIAL_Y, pieceTranslation.getX() * SPECIAL_RATIO, pieceTranslation.getY()) + VecBuilder.fill(SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)) .unit(); double maxDot = -1.0; BranchPositions closestBranch = null; - for (BranchPositions branch : BRANCHES) { + for (int i = 0; i < BRANCHES.length; i++) { Vector branchVec = - branch - .getPosition() - .minus(cameraPos) - .getTranslation() - .rotateBy(invCameraRotation) - .toVector() - .unit(); + Vector.cross(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec), invCameraRotation).unit(); double dot = pieceVec.dot(branchVec); if (dot > maxDot) { maxDot = dot; - closestBranch = branch; + closestBranch = BRANCHES[i]; } } return closestBranch; } - public static AlgaePositions FindAlgae(Pose2d robotPos, Translation2d piecePos) { + public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Translation2d pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); - + final Vector cameraPosVec = cameraPos.getTranslation().toVector(); + final Vector invCameraRotation = cameraPos.getRotation().unaryMinus().toVector(); + final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill(SPECIAL_Y, pieceTranslation.getX() * SPECIAL_RATIO, pieceTranslation.getY()) + VecBuilder.fill(SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)) .unit(); double maxDot = -1.0; AlgaePositions closestAlgae = null; - for (AlgaePositions algae : ALGAES) { + for (int i = 0; i < ALGAES.length; i++) { Vector algaeVec = - algae - .getPosition() - .minus(cameraPos) - .getTranslation() - .rotateBy(invCameraRotation) - .toVector() - .unit(); + Vector.cross(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec), invCameraRotation).unit(); double dot = pieceVec.dot(algaeVec); if (dot > maxDot) { maxDot = dot; - closestAlgae = algae; + closestAlgae = ALGAES[i]; } } return closestAlgae; } + + public static BranchPositions UnitTest1() { + double x = -4.1902085050000 / 3.8337244850000 * SPECIAL_X + Constants.LIMELIGHT_HALF_POS.get(0); + double y = -0.706978325 / 3.8337244850000 * SPECIAL_Y + Constants.LIMELIGHT_HALF_POS.get(1); + return FindCoralBranch( + new Pose2d(0, 0, new Rotation2d(0)), new Vector(VecBuilder.fill(x, y))); + } } diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java index 15a090ce..9e70edff 100644 --- a/src/main/java/frc/robot/commands/UnitTester.java +++ b/src/main/java/frc/robot/commands/UnitTester.java @@ -1,13 +1,10 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utils.shuffleboard.SmartShuffleboard; public class UnitTester extends Command { @Override - public void execute() { - } + public void execute() {} @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index c56ebce8..15a2aa65 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,9 +1,11 @@ package frc.robot.constants; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.utils.SwerveModuleProfileV2; @@ -223,9 +225,10 @@ public enum Mode { public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; // Limelight - public static final Translation2d LIMELIGHT_HALF_POS = new Translation2d(240, 320).div(2); + public static final Vector LIMELIGHT_HALF_POS = + new Vector(VecBuilder.fill(240, 320).div(2)); public static final Transform3d CAMERA_TO_ROBOT = new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later - public static final Translation2d LIMELIGHT_HALF_FOV = - new Translation2d(0.86742863824118179973, 1.04021623418862042785).div(2); + public static final Vector LIMELIGHT_HALF_FOV = + new Vector(VecBuilder.fill(0.86742863824118179973, 1.04021623418862042785).div(2)); } From c27a848691d89094be699d375690311746ae3711 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 12:54:40 -0400 Subject: [PATCH 03/48] find quaternions are better --- .../commands/FindCorrectBranchFromPos.java | 42 +++++++++++-------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 4ad6b4d8..ebdfbb1f 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -14,32 +14,36 @@ public class FindCorrectBranchFromPos { public static final double SPECIAL_X = Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); public static final double SPECIAL_Y = - Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); + Constants.LIMELIGHT_HALF_POS.get(1) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(1)); public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Vector[] PRECOMPUTED_BRANCH_VECS = + private static final Translation3d[] PRECOMPUTED_BRANCH_VECS = Arrays.stream(BranchPositions.values()) - .map(branch -> branch.getPosition().getTranslation().toVector()) - .toArray(Vector[]::new); - private static final Vector[] PRECOMPUTED_ALGAE_VECS = + .map(branch -> branch.getPosition().getTranslation()) + .toArray(Translation3d[]::new); + private static final Translation3d[] PRECOMPUTED_ALGAE_VECS = Arrays.stream(AlgaePositions.values()) - .map(algae -> algae.getPosition().getTranslation().toVector()) - .toArray(Vector[]::new); + .map(algae -> algae.getPosition().getTranslation()) + .toArray(Translation3d[]::new); public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Vector cameraPosVec = cameraPos.getTranslation().toVector(); - final Vector invCameraRotation = cameraPos.getRotation().unaryMinus().toVector(); + final Translation3d cameraPosVec = cameraPos.getTranslation(); + final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill(SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)) - .unit(); + VecBuilder.fill( + SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)); double maxDot = -1.0; BranchPositions closestBranch = null; for (int i = 0; i < BRANCHES.length; i++) { Vector branchVec = - Vector.cross(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec), invCameraRotation).unit(); + PRECOMPUTED_BRANCH_VECS[i] + .minus(cameraPosVec) + .rotateBy(invCameraRotation) + .toVector() + .unit(); double dot = pieceVec.dot(branchVec); if (dot > maxDot) { maxDot = dot; @@ -51,17 +55,21 @@ public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector pieceP public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Vector cameraPosVec = cameraPos.getTranslation().toVector(); - final Vector invCameraRotation = cameraPos.getRotation().unaryMinus().toVector(); + final Translation3d cameraPosVec = cameraPos.getTranslation(); + final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill(SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)) - .unit(); + VecBuilder.fill( + SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)); double maxDot = -1.0; AlgaePositions closestAlgae = null; for (int i = 0; i < ALGAES.length; i++) { Vector algaeVec = - Vector.cross(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec), invCameraRotation).unit(); + PRECOMPUTED_ALGAE_VECS[i] + .minus(cameraPosVec) + .rotateBy(invCameraRotation) + .toVector() + .unit(); double dot = pieceVec.dot(algaeVec); if (dot > maxDot) { maxDot = dot; From 848990ea502ca0c2e37a77f20a1ef3ac632dbb93 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 12:56:04 -0400 Subject: [PATCH 04/48] for loops are better --- .../commands/FindCorrectBranchFromPos.java | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index ebdfbb1f..8d2c860a 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -18,14 +18,21 @@ public class FindCorrectBranchFromPos { public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Translation3d[] PRECOMPUTED_BRANCH_VECS = - Arrays.stream(BranchPositions.values()) - .map(branch -> branch.getPosition().getTranslation()) - .toArray(Translation3d[]::new); - private static final Translation3d[] PRECOMPUTED_ALGAE_VECS = - Arrays.stream(AlgaePositions.values()) - .map(algae -> algae.getPosition().getTranslation()) - .toArray(Translation3d[]::new); + private static final Vector[] PRECOMPUTED_BRANCH_VECS; + private static final Vector[] PRECOMPUTED_ALGAE_VECS; + + static { + PRECOMPUTED_BRANCH_VECS = new Vector[BRANCHES.length]; + for (int i = 0; i < BRANCHES.length; i++) { + PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation().toVector(); + } + + PRECOMPUTED_ALGAE_VECS = new Vector[ALGAES.length]; + for (int i = 0; i < ALGAES.length; i++) { + PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation().toVector(); + } + } + public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); From cc499ae5a4beddaca34e84aed634e36c98a2f61a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 12:59:03 -0400 Subject: [PATCH 05/48] fixed --- .../frc/robot/commands/FindCorrectBranchFromPos.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 8d2c860a..c3cd12fe 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -18,18 +18,18 @@ public class FindCorrectBranchFromPos { public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Vector[] PRECOMPUTED_BRANCH_VECS; - private static final Vector[] PRECOMPUTED_ALGAE_VECS; + private static final Translation3d[] PRECOMPUTED_BRANCH_VECS; + private static final Translation3d[] PRECOMPUTED_ALGAE_VECS; static { - PRECOMPUTED_BRANCH_VECS = new Vector[BRANCHES.length]; + PRECOMPUTED_BRANCH_VECS = new Translation3d[BRANCHES.length]; for (int i = 0; i < BRANCHES.length; i++) { - PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation().toVector(); + PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation(); } - PRECOMPUTED_ALGAE_VECS = new Vector[ALGAES.length]; + PRECOMPUTED_ALGAE_VECS = new Translation3d[ALGAES.length]; for (int i = 0; i < ALGAES.length; i++) { - PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation().toVector(); + PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation(); } } From 99f5a8c1d3e487644693fc43fbf2d097f21bfb44 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 13:03:14 -0400 Subject: [PATCH 06/48] linter --- src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index c3cd12fe..44368e8d 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -8,7 +8,6 @@ import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; -import java.util.Arrays; public class FindCorrectBranchFromPos { public static final double SPECIAL_X = @@ -33,7 +32,6 @@ public class FindCorrectBranchFromPos { } } - public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); From 49d196773b2faddb534ac98d03049938f25efb35 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 16:58:50 -0400 Subject: [PATCH 07/48] corrected for api --- .../commands/FindCorrectBranchFromPos.java | 22 +++++++------------ .../java/frc/robot/commands/UnitTester.java | 5 ++++- .../frc/robot/constants/GameConstants.java | 2 +- 3 files changed, 13 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 44368e8d..2ab37a8d 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -10,11 +10,6 @@ import frc.robot.constants.Constants; public class FindCorrectBranchFromPos { - public static final double SPECIAL_X = - Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); - public static final double SPECIAL_Y = - Constants.LIMELIGHT_HALF_POS.get(1) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(1)); - public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); private static final Translation3d[] PRECOMPUTED_BRANCH_VECS; @@ -32,14 +27,13 @@ public class FindCorrectBranchFromPos { } } + // PeicePos is in degrees tx,ty public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill( - SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)); + VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; BranchPositions closestBranch = null; for (int i = 0; i < BRANCHES.length; i++) { @@ -58,14 +52,13 @@ public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector pieceP return closestBranch; } + // peicepos is in digrees tx,ty public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill( - SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)); + VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; AlgaePositions closestAlgae = null; for (int i = 0; i < ALGAES.length; i++) { @@ -85,9 +78,10 @@ public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { } public static BranchPositions UnitTest1() { - double x = -4.1902085050000 / 3.8337244850000 * SPECIAL_X + Constants.LIMELIGHT_HALF_POS.get(0); - double y = -0.706978325 / 3.8337244850000 * SPECIAL_Y + Constants.LIMELIGHT_HALF_POS.get(1); + double x = 120; + double y = 116.49122807017543859649; return FindCoralBranch( - new Pose2d(0, 0, new Rotation2d(0)), new Vector(VecBuilder.fill(x, y))); + new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), + new Vector(VecBuilder.fill(x, y))); } } diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java index 9e70edff..99d00477 100644 --- a/src/main/java/frc/robot/commands/UnitTester.java +++ b/src/main/java/frc/robot/commands/UnitTester.java @@ -1,10 +1,13 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utils.shuffleboard.SmartShuffleboard; public class UnitTester extends Command { @Override - public void execute() {} + public void execute() { + SmartShuffleboard.put("Branch", "Branch", FindCorrectBranchFromPos.UnitTest1().toString()); + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 15a2aa65..ce85b575 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -228,7 +228,7 @@ public enum Mode { public static final Vector LIMELIGHT_HALF_POS = new Vector(VecBuilder.fill(240, 320).div(2)); public static final Transform3d CAMERA_TO_ROBOT = - new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later + new Transform3d(0.0, 0.0, 0.55, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later public static final Vector LIMELIGHT_HALF_FOV = new Vector(VecBuilder.fill(0.86742863824118179973, 1.04021623418862042785).div(2)); } From 49da7b6c9218b1deeda7573dacf9e393e06d8c81 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 18:27:43 -0400 Subject: [PATCH 08/48] fixed some sutff --- .../frc/robot/commands/FindCorrectBranchFromPos.java | 10 +++++----- src/main/java/frc/robot/constants/GameConstants.java | 7 ------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 2ab37a8d..6d3d22f7 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -27,13 +27,13 @@ public class FindCorrectBranchFromPos { } } - // PeicePos is in degrees tx,ty + // PeicePos is in Radians public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); + VecBuilder.fill(1, Math.tan(piecePos.get(1)), Math.tan(piecePos.get(0))); double maxDot = -1.0; BranchPositions closestBranch = null; for (int i = 0; i < BRANCHES.length; i++) { @@ -52,7 +52,7 @@ public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector pieceP return closestBranch; } - // peicepos is in digrees tx,ty + // peicepos is in Radians public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); @@ -78,8 +78,8 @@ public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { } public static BranchPositions UnitTest1() { - double x = 120; - double y = 116.49122807017543859649; + double x = 0.84151552345920325992; + double y = 0; return FindCoralBranch( new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), new Vector(VecBuilder.fill(x, y))); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index ce85b575..839ce57d 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,11 +1,8 @@ package frc.robot.constants; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.utils.SwerveModuleProfileV2; @@ -225,10 +222,6 @@ public enum Mode { public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; // Limelight - public static final Vector LIMELIGHT_HALF_POS = - new Vector(VecBuilder.fill(240, 320).div(2)); public static final Transform3d CAMERA_TO_ROBOT = new Transform3d(0.0, 0.0, 0.55, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later - public static final Vector LIMELIGHT_HALF_FOV = - new Vector(VecBuilder.fill(0.86742863824118179973, 1.04021623418862042785).div(2)); } From da456e916b1b9c92911d25f0cb99635692db2413 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 18:39:23 -0400 Subject: [PATCH 09/48] made code more maintanable and stuff --- .../commands/FindCorrectBranchFromPos.java | 51 +++++++------------ 1 file changed, 17 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 6d3d22f7..85a908e5 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -29,52 +29,35 @@ public class FindCorrectBranchFromPos { // PeicePos is in Radians public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { + return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); + } + public static > T findClosestPosition (Pose2d robotPos, Vector piecePos, T[] values, Translation3d[] preComputedVecs) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(piecePos.get(1)), Math.tan(piecePos.get(0))); + VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; - BranchPositions closestBranch = null; - for (int i = 0; i < BRANCHES.length; i++) { - Vector branchVec = - PRECOMPUTED_BRANCH_VECS[i] - .minus(cameraPosVec) - .rotateBy(invCameraRotation) - .toVector() - .unit(); - double dot = pieceVec.dot(branchVec); + T closest = null; + for (int i = 0; i < values.length; i++) { + Vector locationVec = + preComputedVecs[i] + .minus(cameraPosVec) + .rotateBy(invCameraRotation) + .toVector() + .unit(); + double dot = pieceVec.dot(locationVec); if (dot > maxDot) { maxDot = dot; - closestBranch = BRANCHES[i]; + closest = values[i]; } } - return closestBranch; + return closest; } // peicepos is in Radians public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { - final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Translation3d cameraPosVec = cameraPos.getTranslation(); - final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); - double maxDot = -1.0; - AlgaePositions closestAlgae = null; - for (int i = 0; i < ALGAES.length; i++) { - Vector algaeVec = - PRECOMPUTED_ALGAE_VECS[i] - .minus(cameraPosVec) - .rotateBy(invCameraRotation) - .toVector() - .unit(); - double dot = pieceVec.dot(algaeVec); - if (dot > maxDot) { - maxDot = dot; - closestAlgae = ALGAES[i]; - } - } - return closestAlgae; + return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); } public static BranchPositions UnitTest1() { @@ -82,6 +65,6 @@ public static BranchPositions UnitTest1() { double y = 0; return FindCoralBranch( new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), - new Vector(VecBuilder.fill(x, y))); + VecBuilder.fill(x, y)); } } From 2e4df64398962fb6a7737ed362d000297cc3cb47 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 19:17:02 -0400 Subject: [PATCH 10/48] all linear algebra now --- .../commands/FindCorrectBranchFromPos.java | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 85a908e5..5fbfcc3b 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -1,8 +1,10 @@ package frc.robot.commands; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N3; import frc.robot.constants.AlgaePositions; @@ -12,18 +14,18 @@ public class FindCorrectBranchFromPos { private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Translation3d[] PRECOMPUTED_BRANCH_VECS; - private static final Translation3d[] PRECOMPUTED_ALGAE_VECS; + private static final Vector[] PRECOMPUTED_BRANCH_VECS; + private static final Vector[] PRECOMPUTED_ALGAE_VECS; static { - PRECOMPUTED_BRANCH_VECS = new Translation3d[BRANCHES.length]; + PRECOMPUTED_BRANCH_VECS = new Vector[BRANCHES.length]; for (int i = 0; i < BRANCHES.length; i++) { - PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation(); + PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation().toVector(); } - PRECOMPUTED_ALGAE_VECS = new Translation3d[ALGAES.length]; + PRECOMPUTED_ALGAE_VECS = new Vector[ALGAES.length]; for (int i = 0; i < ALGAES.length; i++) { - PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation(); + PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation().toVector(); } } @@ -31,22 +33,19 @@ public class FindCorrectBranchFromPos { public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); } - public static > T findClosestPosition (Pose2d robotPos, Vector piecePos, T[] values, Translation3d[] preComputedVecs) { + public static > T findClosestPosition (Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Translation3d cameraPosVec = cameraPos.getTranslation(); - final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); + final Vector cameraPosVec = cameraPos.getTranslation().toVector(); + final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { - Vector locationVec = + Matrix locationVec =(invCameraRotation.times( preComputedVecs[i] - .minus(cameraPosVec) - .rotateBy(invCameraRotation) - .toVector() - .unit(); - double dot = pieceVec.dot(locationVec); + .minus(cameraPosVec))); + double dot = pieceVec.dot((Vector) locationVec); if (dot > maxDot) { maxDot = dot; closest = values[i]; From f64921482b2d891d6a9e0c725de4d644b22212e2 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 19:17:49 -0400 Subject: [PATCH 11/48] linter --- .../robot/commands/FindCorrectBranchFromPos.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 5fbfcc3b..9654b4c4 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -33,18 +33,19 @@ public class FindCorrectBranchFromPos { public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); } - public static > T findClosestPosition (Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { + + public static > T findClosestPosition( + Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); + VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { - Matrix locationVec =(invCameraRotation.times( - preComputedVecs[i] - .minus(cameraPosVec))); + Matrix locationVec = + (invCameraRotation.times(preComputedVecs[i].minus(cameraPosVec))); double dot = pieceVec.dot((Vector) locationVec); if (dot > maxDot) { maxDot = dot; @@ -63,7 +64,6 @@ public static BranchPositions UnitTest1() { double x = 0.84151552345920325992; double y = 0; return FindCoralBranch( - new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), - VecBuilder.fill(x, y)); + new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), VecBuilder.fill(x, y)); } } From fd5830a42c775bd21e42411752558b1581a02424 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 19:26:08 -0400 Subject: [PATCH 12/48] more generilization --- .../commands/FindCorrectBranchFromPos.java | 27 +++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 9654b4c4..d7b91b6a 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -14,19 +14,24 @@ public class FindCorrectBranchFromPos { private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Vector[] PRECOMPUTED_BRANCH_VECS; - private static final Vector[] PRECOMPUTED_ALGAE_VECS; + // Precomputed vectors for positions + private static final Vector[] PRECOMPUTED_BRANCH_VECS = precomputePositionVectors(BRANCHES); + private static final Vector[] PRECOMPUTED_ALGAE_VECS = precomputePositionVectors(ALGAES); - static { - PRECOMPUTED_BRANCH_VECS = new Vector[BRANCHES.length]; - for (int i = 0; i < BRANCHES.length; i++) { - PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation().toVector(); - } - - PRECOMPUTED_ALGAE_VECS = new Vector[ALGAES.length]; - for (int i = 0; i < ALGAES.length; i++) { - PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation().toVector(); + // Precompute position vectors for any enum type with a getPosition method + @SuppressWarnings("unchecked") + private static > Vector[] precomputePositionVectors(T[] values) { + Vector[] vectors = new Vector[values.length]; + for (int i = 0; i < values.length; i++) { + // Using reflection to access getPosition method - assumes the method exists on the enum + try { + Pose3d pose = (Pose3d) values[i].getClass().getMethod("getPosition").invoke(values[i]); + vectors[i] = pose.getTranslation().toVector(); + } catch (Exception e) { + throw new RuntimeException("Failed to precompute position vectors", e); + } } + return vectors; } // PeicePos is in Radians From 129217270a33cbe21aaa42766e4e096d21222866 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 19:44:44 -0400 Subject: [PATCH 13/48] EA SPORTS --- .../java/frc/robot/commands/FindCorrectBranchFromPos.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index d7b91b6a..3b1bcb7b 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -45,13 +45,16 @@ public static > T findClosestPosition( final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); + VecBuilder.fill(1, Math.tan(-piecePos.get(1)), Math.tan(piecePos.get(0))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { Matrix locationVec = (invCameraRotation.times(preComputedVecs[i].minus(cameraPosVec))); - double dot = pieceVec.dot((Vector) locationVec); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); if (dot > maxDot) { maxDot = dot; closest = values[i]; From 47fe8e05c29971a77d6e621851b4f138f00735df Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 11:27:34 -0400 Subject: [PATCH 14/48] fixed --- .../frc/robot/commands/FindCorrectBranchFromPos.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 3b1bcb7b..cdd2a03e 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -45,7 +45,7 @@ public static > T findClosestPosition( final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(1)), Math.tan(piecePos.get(0))); + VecBuilder.fill(1, Math.tan(piecePos.get(1)), Math.tan(piecePos.get(0))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { @@ -68,10 +68,10 @@ public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); } - public static BranchPositions UnitTest1() { - double x = 0.84151552345920325992; - double y = 0; - return FindCoralBranch( - new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), VecBuilder.fill(x, y)); + public static AlgaePositions UnitTest1() { + double x = 0.31119220563058896357; + double y = 0.06719517620178168871; + return FindAlgae( + new Pose2d(1.453724485, 3.8615914950000, new Rotation2d(0)), VecBuilder.fill(x, y)); } } From a3024518a625aa7fadfbc7639ae318262303a23a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 12:22:02 -0400 Subject: [PATCH 15/48] added limelight --- .../frc/robot/constants/GameConstants.java | 1 + .../subsystems/limelight/MockVisionIO.java | 6 ++ .../subsystems/limelight/RealVisionIO.java | 56 +++++++++++++++++++ .../robot/subsystems/limelight/Vision.java | 53 ++++++++++++++++++ .../robot/subsystems/limelight/VisionIO.java | 5 ++ .../subsystems/limelight/VisionInputs.java | 40 +++++++++++++ .../frc/robot/utils/diag/DiagLimelight.java | 47 ++++++++++++++++ 7 files changed, 208 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/limelight/Vision.java create mode 100644 src/main/java/frc/robot/subsystems/limelight/VisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/limelight/VisionInputs.java create mode 100644 src/main/java/frc/robot/utils/diag/DiagLimelight.java diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 839ce57d..0c648888 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -224,4 +224,5 @@ public enum Mode { // Limelight public static final Transform3d CAMERA_TO_ROBOT = new Transform3d(0.0, 0.0, 0.55, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later + public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.36"; // TODO Change Later } diff --git a/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java new file mode 100644 index 00000000..634f7193 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java @@ -0,0 +1,6 @@ +package frc.robot.subsystems.limelight; + +public class MockVisionIO implements VisionIO { + @Override + public void updateInputs(VisionInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java new file mode 100644 index 00000000..a7c00a70 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.limelight; + +import edu.wpi.first.cscore.HttpCamera; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import frc.robot.Robot; +import frc.robot.commands.FindCorrectBranchFromPos; +import frc.robot.constants.Constants; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.diag.DiagLimelight; +import java.util.Map; + +public class RealVisionIO implements VisionIO { + private final NetworkTableEntry tv; + private final NetworkTableEntry tx; + private final NetworkTableEntry ty; + private final SwerveDrivetrain drivetrain; + + public RealVisionIO(SwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + tv = table.getEntry("tv"); + tx = table.getEntry("tx"); + ty = table.getEntry("ty"); + Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); + + HttpCamera limelightFeed = + new HttpCamera( + "limelight", "http://" + Constants.LIMELIGHT_IP_ADDRESS + ":5800/stream.mjpg"); + ShuffleboardTab dashboardTab = Shuffleboard.getTab("Driver"); + dashboardTab + .add("Limelight feed", limelightFeed) + .withSize(6, 4) + .withPosition(2, 0) + .withProperties(Map.of("Show Crosshair", false, "Show Controls", false)); + } + + @Override + public void updateInputs(VisionInputs inputs) { + inputs.tv = tv.getDouble(0); + inputs.tx = tx.getDouble(0); + inputs.ty = ty.getDouble(0); + if (inputs.tv != 0) { + inputs.coralSeen.add( + FindCorrectBranchFromPos.FindCoralBranch( + drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); + inputs.algaeSeen.add( + FindCorrectBranchFromPos.FindAlgae( + drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java new file mode 100644 index 00000000..14a8432b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -0,0 +1,53 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.limelight; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.AlgaePositions; +import frc.robot.constants.BranchPositions; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.subsystem.LoggableSystem; +import java.util.ArrayList; + +public class Vision extends SubsystemBase { + LoggableSystem system; + SwerveDrivetrain drivetrain; + + public Vision(VisionIO io, SwerveDrivetrain drivetrain) { + system = new LoggableSystem<>(io, new VisionInputs("limelight")); + this.drivetrain = drivetrain; + } + + /** + * @return The piece's x offset angle in degrees and 0.0 if the piece isn't seen + */ + public double getPieceOffsetAngleX() { + return system.getInputs().tx; + } + + /** + * @return The piece's y offset angle in degrees and 0.0 if the piece isn't seen + */ + public double getPieceOffsetAngleY() { + return system.getInputs().ty; + } + + public boolean isPieceSeen() { + return system.getInputs().tv != 0; + } + + public ArrayList getAllBranchPosition() { + return system.getInputs().coralSeen; + } + + public ArrayList getAllAlgaePosition() { + return system.getInputs().algaeSeen; + } + + @Override + public void periodic() { + system.updateInputs(); + } +} diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionIO.java b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java new file mode 100644 index 00000000..990d52a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.limelight; + +import frc.robot.utils.logging.LoggableIO; + +public interface VisionIO extends LoggableIO {} diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java new file mode 100644 index 00000000..c06e640f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.limelight; + +import frc.robot.constants.AlgaePositions; +import frc.robot.constants.BranchPositions; +import frc.robot.utils.logging.subsystem.FolderLoggableInputs; +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.LogTable; + +public class VisionInputs extends FolderLoggableInputs { + public double tv = 0; + public double tx = 0; + public double ty = 0; + public ArrayList coralSeen; + public ArrayList algaeSeen; + + public VisionInputs(String folder) { + super(folder); + } + + @Override + public void toLog(LogTable table) { + table.put("tv", tv); + table.put("tx", tx); + table.put("ty", ty); + table.put("coralSeen", coralSeen.toArray(BranchPositions[]::new)); + table.put("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)); + } + + @Override + public void fromLog(LogTable table) { + tv = table.get("tv", tv); + tx = table.get("tx", tx); + ty = table.get("ty", ty); + coralSeen = + new ArrayList<>(List.of(table.get("coralSeen", coralSeen.toArray(BranchPositions[]::new)))); + algaeSeen = + new ArrayList<>(List.of(table.get("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)))); + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagLimelight.java b/src/main/java/frc/robot/utils/diag/DiagLimelight.java new file mode 100644 index 00000000..ad4fbdd6 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagLimelight.java @@ -0,0 +1,47 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +public class DiagLimelight implements Diagnosable { + private DoubleSubscriber subscriber; + private String title; + private String name; + private GenericEntry networkTableEntry; + + public DiagLimelight(String title, String name) { + this.title = title; + this.name = name; + reset(); + } + + protected boolean getDiagResult() { + return subscriber.get() == 1; + } + + @Override + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = + shuffleBoardTab + .getLayout(title, BuiltInLayouts.kList) + .withSize(width, height) + .add(name, false) + .getEntry(); + } + + @Override + public void refresh() { + if (networkTableEntry != null) { + networkTableEntry.setBoolean(getDiagResult()); + } + } + + @Override + public void reset() { + subscriber = + NetworkTableInstance.getDefault().getTable("limelight").getDoubleTopic("tv").subscribe(-1); + } +} From baca8c98f45c979014c62b213a8d26c72ecb0b74 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 16:39:46 -0400 Subject: [PATCH 16/48] small changes --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ .../frc/robot/commands/FindCorrectBranchFromPos.java | 5 ++--- src/main/java/frc/robot/constants/GameConstants.java | 7 ++++--- .../java/frc/robot/subsystems/limelight/Vision.java | 12 ++++++++---- .../frc/robot/subsystems/limelight/VisionInputs.java | 8 ++++++-- 5 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 51287d68..363b91d4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,6 +63,8 @@ import frc.robot.subsystems.lightStrip.LightStrip; import frc.robot.subsystems.lightStrip.MockLightStripIO; import frc.robot.subsystems.lightStrip.RealLightStripIO; +import frc.robot.subsystems.limelight.RealVisionIO; +import frc.robot.subsystems.limelight.Vision; import frc.robot.subsystems.swervev3.KinematicsConversionConfig; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.subsystems.swervev3.SwerveIdConfig; @@ -105,6 +107,7 @@ public class RobotContainer { private final Joystick joyright = new Joystick(Constants.RIGHT_JOYSTICK_ID); private RobotVisualizer robotVisualizer = null; private SwerveDriveSimulation driveSimulation; + private final Vision vision; public RobotContainer() { switch (Constants.currentMode) { @@ -163,6 +166,7 @@ public RobotContainer() { } } setupDriveTrain(); + vision = new Vision(new RealVisionIO(drivetrain)); configureBindings(); setupAutoChooser(); putShuffleboardCommands(); diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index cdd2a03e..e76b8e7a 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -45,7 +45,7 @@ public static > T findClosestPosition( final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(piecePos.get(1)), Math.tan(piecePos.get(0))); + VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { @@ -71,7 +71,6 @@ public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { public static AlgaePositions UnitTest1() { double x = 0.31119220563058896357; double y = 0.06719517620178168871; - return FindAlgae( - new Pose2d(1.453724485, 3.8615914950000, new Rotation2d(0)), VecBuilder.fill(x, y)); + return FindAlgae(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 0c648888..1d05fd37 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -90,7 +90,7 @@ public class GameConstants { public static final int HIHI_EXTENDER_TICK_LIMIT = 10; // Mode - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.REAL; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; public enum Mode { @@ -223,6 +223,7 @@ public enum Mode { // Limelight public static final Transform3d CAMERA_TO_ROBOT = - new Transform3d(0.0, 0.0, 0.55, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later - public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.36"; // TODO Change Later + new Transform3d( + 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later + public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later } diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 14a8432b..786636db 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -7,17 +7,15 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.subsystem.LoggableSystem; +import frc.robot.utils.shuffleboard.SmartShuffleboard; import java.util.ArrayList; public class Vision extends SubsystemBase { LoggableSystem system; - SwerveDrivetrain drivetrain; - public Vision(VisionIO io, SwerveDrivetrain drivetrain) { + public Vision(VisionIO io) { system = new LoggableSystem<>(io, new VisionInputs("limelight")); - this.drivetrain = drivetrain; } /** @@ -49,5 +47,11 @@ public ArrayList getAllAlgaePosition() { @Override public void periodic() { system.updateInputs(); + if (getAllAlgaePosition() != null) { + SmartShuffleboard.put("peice pos", "Algae", getAllAlgaePosition()); + } + if (getAllBranchPosition() != null) { + SmartShuffleboard.put("peice pos", "Coral", getAllBranchPosition()); + } } } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java index c06e640f..908d6eec 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -23,8 +23,12 @@ public void toLog(LogTable table) { table.put("tv", tv); table.put("tx", tx); table.put("ty", ty); - table.put("coralSeen", coralSeen.toArray(BranchPositions[]::new)); - table.put("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)); + if (coralSeen != null) { + table.put("coralSeen", coralSeen.toArray(BranchPositions[]::new)); + } + if (algaeSeen != null) { + table.put("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)); + } } @Override From a2cdbdcec8c901211b3b2ef909976956c287c944 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 18:59:40 -0400 Subject: [PATCH 17/48] fixed reflecton issue --- ...ranchFromPos.java => GamePieceLocate.java} | 36 ++++++++----------- .../java/frc/robot/commands/UnitTester.java | 2 +- .../subsystems/limelight/RealVisionIO.java | 6 ++-- 3 files changed, 19 insertions(+), 25 deletions(-) rename src/main/java/frc/robot/commands/{FindCorrectBranchFromPos.java => GamePieceLocate.java} (62%) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/GamePieceLocate.java similarity index 62% rename from src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java rename to src/main/java/frc/robot/commands/GamePieceLocate.java index e76b8e7a..3fc056c8 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -11,31 +11,25 @@ import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; -public class FindCorrectBranchFromPos { +import java.util.Arrays; + +public class GamePieceLocate { private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); // Precomputed vectors for positions - private static final Vector[] PRECOMPUTED_BRANCH_VECS = precomputePositionVectors(BRANCHES); - private static final Vector[] PRECOMPUTED_ALGAE_VECS = precomputePositionVectors(ALGAES); + private static final Vector[] PRECOMPUTED_BRANCH_VECS = Arrays.stream(BranchPositions.values()) + .map(branch -> branch.getPosition().getTranslation().toVector()) + .toArray(Vector[]::new);; + + private static final Vector[] PRECOMPUTED_ALGAE_VECS = Arrays.stream(AlgaePositions.values()) + .map(algae -> algae.getPosition().getTranslation().toVector()) + .toArray(Vector[]::new); + + - // Precompute position vectors for any enum type with a getPosition method - @SuppressWarnings("unchecked") - private static > Vector[] precomputePositionVectors(T[] values) { - Vector[] vectors = new Vector[values.length]; - for (int i = 0; i < values.length; i++) { - // Using reflection to access getPosition method - assumes the method exists on the enum - try { - Pose3d pose = (Pose3d) values[i].getClass().getMethod("getPosition").invoke(values[i]); - vectors[i] = pose.getTranslation().toVector(); - } catch (Exception e) { - throw new RuntimeException("Failed to precompute position vectors", e); - } - } - return vectors; - } // PeicePos is in Radians - public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { + public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); } @@ -64,13 +58,13 @@ public static > T findClosestPosition( } // peicepos is in Radians - public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { + public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); } public static AlgaePositions UnitTest1() { double x = 0.31119220563058896357; double y = 0.06719517620178168871; - return FindAlgae(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); + return findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); } } diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java index 99d00477..1ba6c7cc 100644 --- a/src/main/java/frc/robot/commands/UnitTester.java +++ b/src/main/java/frc/robot/commands/UnitTester.java @@ -6,7 +6,7 @@ public class UnitTester extends Command { @Override public void execute() { - SmartShuffleboard.put("Branch", "Branch", FindCorrectBranchFromPos.UnitTest1().toString()); + SmartShuffleboard.put("Branch", "Branch", GamePieceLocate.UnitTest1().toString()); } @Override diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index a7c00a70..04692537 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; -import frc.robot.commands.FindCorrectBranchFromPos; +import frc.robot.commands.GamePieceLocate; import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.diag.DiagLimelight; @@ -46,10 +46,10 @@ public void updateInputs(VisionInputs inputs) { inputs.ty = ty.getDouble(0); if (inputs.tv != 0) { inputs.coralSeen.add( - FindCorrectBranchFromPos.FindCoralBranch( + GamePieceLocate.findCoralBranch( drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); inputs.algaeSeen.add( - FindCorrectBranchFromPos.FindAlgae( + GamePieceLocate.findAlgaePos( drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); } } From 6cf0016798871d401610b93e3509c16cc00af8d1 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 19:00:38 -0400 Subject: [PATCH 18/48] fixed reflecton issue --- .../frc/robot/commands/GamePieceLocate.java | 13 ++++----- .../robot/commands/climber/StopClimber.java | 29 ++++++++++--------- .../robot/commands/sequences/CancelAll.java | 9 +++--- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/commands/GamePieceLocate.java index 3fc056c8..bfbbefb1 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -10,24 +10,23 @@ import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; - import java.util.Arrays; public class GamePieceLocate { private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); // Precomputed vectors for positions - private static final Vector[] PRECOMPUTED_BRANCH_VECS = Arrays.stream(BranchPositions.values()) + private static final Vector[] PRECOMPUTED_BRANCH_VECS = + Arrays.stream(BranchPositions.values()) .map(branch -> branch.getPosition().getTranslation().toVector()) - .toArray(Vector[]::new);; + .toArray(Vector[]::new); + ; - private static final Vector[] PRECOMPUTED_ALGAE_VECS = Arrays.stream(AlgaePositions.values()) + private static final Vector[] PRECOMPUTED_ALGAE_VECS = + Arrays.stream(AlgaePositions.values()) .map(algae -> algae.getPosition().getTranslation().toVector()) .toArray(Vector[]::new); - - - // PeicePos is in Radians public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); diff --git a/src/main/java/frc/robot/commands/climber/StopClimber.java b/src/main/java/frc/robot/commands/climber/StopClimber.java index eae2a033..2811e498 100644 --- a/src/main/java/frc/robot/commands/climber/StopClimber.java +++ b/src/main/java/frc/robot/commands/climber/StopClimber.java @@ -4,17 +4,20 @@ import frc.robot.utils.logging.commands.LoggableCommand; public class StopClimber extends LoggableCommand { - private ClimberSubsystem climber; - public StopClimber(ClimberSubsystem climber) { - this.climber = climber; - addRequirements(climber); - } - @Override - public void initialize() { - climber.stopClimber(); - } - @Override - public boolean isFinished() { - return true; - } + private ClimberSubsystem climber; + + public StopClimber(ClimberSubsystem climber) { + this.climber = climber; + addRequirements(climber); + } + + @Override + public void initialize() { + climber.stopClimber(); + } + + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/frc/robot/commands/sequences/CancelAll.java b/src/main/java/frc/robot/commands/sequences/CancelAll.java index 476efc4d..745b0628 100644 --- a/src/main/java/frc/robot/commands/sequences/CancelAll.java +++ b/src/main/java/frc/robot/commands/sequences/CancelAll.java @@ -10,17 +10,16 @@ import frc.robot.subsystems.algaebyebyetilt.AlgaeByeByeTiltSubsystem; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; /** Add your docs here. */ public class CancelAll extends LoggableSequentialCommandGroup { public CancelAll( - AlgaeByeByeTiltSubsystem algaeByeByeTiltSubsystem, - AlgaeByeByeRollerSubsystem algaebyebyeroller, - ElevatorSubsystem elevatorSubsystem, - ClimberSubsystem climberSubsystem) { + AlgaeByeByeTiltSubsystem algaeByeByeTiltSubsystem, + AlgaeByeByeRollerSubsystem algaebyebyeroller, + ElevatorSubsystem elevatorSubsystem, + ClimberSubsystem climberSubsystem) { super( new StopClimber(climberSubsystem), new ByeByeAllDone(algaeByeByeTiltSubsystem, algaebyebyeroller, elevatorSubsystem), From 69cdd68d3d4965609baad53283a0554268fe6d67 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 19:02:59 -0400 Subject: [PATCH 19/48] fixed reflecton issue --- src/main/java/frc/robot/commands/GamePieceLocate.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/commands/GamePieceLocate.java index bfbbefb1..b235ecc9 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -17,17 +17,17 @@ public class GamePieceLocate { private static final AlgaePositions[] ALGAES = AlgaePositions.values(); // Precomputed vectors for positions private static final Vector[] PRECOMPUTED_BRANCH_VECS = - Arrays.stream(BranchPositions.values()) + Arrays.stream(BRANCHES) .map(branch -> branch.getPosition().getTranslation().toVector()) .toArray(Vector[]::new); ; private static final Vector[] PRECOMPUTED_ALGAE_VECS = - Arrays.stream(AlgaePositions.values()) + Arrays.stream(ALGAES) .map(algae -> algae.getPosition().getTranslation().toVector()) .toArray(Vector[]::new); - // PeicePos is in Radians + // Piece Pos is in Radians public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); } @@ -56,7 +56,7 @@ public static > T findClosestPosition( return closest; } - // peicepos is in Radians + // piece pos is in Radians public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); } From 8f331ef9a567e761dcf0f47befdcaf4ee3552e2c Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 21:37:34 -0400 Subject: [PATCH 20/48] small pr fixed --- src/main/java/frc/robot/commands/GamePieceLocate.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/commands/GamePieceLocate.java index b235ecc9..3c201ef2 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -34,7 +34,7 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP public static > T findClosestPosition( Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { - final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); + final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 1d05fd37..3654cac6 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -222,7 +222,7 @@ public enum Mode { public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; // Limelight - public static final Transform3d CAMERA_TO_ROBOT = + public static final Transform3d LIMELIGHT_TO_ROBOT = new Transform3d( 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later From 0a0dbe76127b43dd28dd3566da1d80b7a4f14c4a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 24 Mar 2025 11:56:02 -0400 Subject: [PATCH 21/48] made only check closest and neighboring algae, coral. --- .../frc/robot/commands/GamePieceLocate.java | 155 ++++++++++++++++-- .../frc/robot/constants/CenterPositions.java | 36 ++++ 2 files changed, 174 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc/robot/constants/CenterPositions.java diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/commands/GamePieceLocate.java index 3c201ef2..230fd1ee 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.numbers.N3; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; +import frc.robot.constants.CenterPositions; import frc.robot.constants.Constants; import java.util.Arrays; @@ -29,28 +30,76 @@ public class GamePieceLocate { // Piece Pos is in Radians public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { - return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); - } - - public static > T findClosestPosition( - Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); double maxDot = -1.0; - T closest = null; - for (int i = 0; i < values.length; i++) { - Matrix locationVec = - (invCameraRotation.times(preComputedVecs[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = values[i]; + BranchPositions closest = null; + int n = CenterPositions.getClosest(robotPos); + if (n == 0) { + for (int i = 0; i < 9; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } + } + for (int i = 33; i < 36; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } + } + } else if (n == 5) { + for (int i = 0; i < 3; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } + } + for (int i = 6 * n - 3; i < 6 * n + 6; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } + } + } else { + for (int i = 6 * n - 3; i < 6 * n + 9; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } } } return closest; @@ -58,7 +107,79 @@ public static > T findClosestPosition( // piece pos is in Radians public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) { - return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); + final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); + final Vector cameraPosVec = cameraPos.getTranslation().toVector(); + final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); + final Vector pieceVec = + VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); + double maxDot = -1.0; + AlgaePositions closest = null; + int n = CenterPositions.getClosest(robotPos); + if (n == 0) { + for (int i = 0; i < 6; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + for (int i = 10; i < 12; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + } else if (n == 5) { + for (int i = 0; i < 3; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + for (int i = 8; i < 12; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + } else { + for (int i = 2 * n - 2; i < 2 * n + 4; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + } + return closest; } public static AlgaePositions UnitTest1() { diff --git a/src/main/java/frc/robot/constants/CenterPositions.java b/src/main/java/frc/robot/constants/CenterPositions.java new file mode 100644 index 00000000..fd6746f4 --- /dev/null +++ b/src/main/java/frc/robot/constants/CenterPositions.java @@ -0,0 +1,36 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; + +public enum CenterPositions { + CENTER_AB(new Translation2d(3.8337244850000, 4.0259000000000)), + CENTER_CD(new Translation2d(4.1615306625000, 3.4581230455351)), + CENTER_EF(new Translation2d(4.8171430175000, 3.4581230455351)), + CENTER_GH(new Translation2d(5.1449491950000, 4.0259000000000)), + CENTER_IJ(new Translation2d(4.8171430175000, 4.5936769544649)), + CENTER_KL(new Translation2d(4.1615306625000, 4.5936769544649)); + + private final Translation2d position; + + CenterPositions(Translation2d position) { + this.position = position; + } + + public Translation2d getPosition() { + return position; + } + + public static int getClosest(Pose2d robotPose) { + double closeDistance = 100; + int n = 0; + for (int i = 0; i < values().length; i++) { + double distance = robotPose.getTranslation().getDistance(values()[i].getPosition()); + if (distance < closeDistance) { + n = i; + closeDistance = distance; + } + } + return n; + } +} From 61dc0a2c46cb1d986d9201f201a6f2d9084a334e Mon Sep 17 00:00:00 2001 From: Code Toad <55628278+goatfanboi23@users.noreply.github.com> Date: Mon, 24 Mar 2025 11:58:44 -0400 Subject: [PATCH 22/48] made unit tests (#295) Co-authored-by: codetoad Co-authored-by: Lev Strougov <62769580+Levercpu@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 3 --- .../java/frc/robot/commands/UnitTester.java | 16 ------------- .../subsystems/limelight/RealVisionIO.java | 2 +- .../{commands => utils}/GamePieceLocate.java | 8 +------ .../frc/robot/utils/GamePieceLocateTest.java | 23 +++++++++++++++++++ 5 files changed, 25 insertions(+), 27 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/UnitTester.java rename src/main/java/frc/robot/{commands => utils}/GamePieceLocate.java (96%) create mode 100644 src/test/java/frc/robot/utils/GamePieceLocateTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cffea4f6..de9e5ce5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,7 +22,6 @@ import frc.robot.autochooser.FieldLocation; import frc.robot.autochooser.chooser.AutoChooser2025; import frc.robot.autochooser.event.RealAutoEventProvider; -import frc.robot.commands.UnitTester; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; import frc.robot.commands.coral.IntakeCoral; @@ -80,7 +79,6 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; -import frc.robot.utils.shuffleboard.SmartShuffleboard; import frc.robot.utils.simulation.RobotVisualizer; import frc.robot.utils.simulation.SwerveSimulationUtils; import java.util.function.Consumer; @@ -445,7 +443,6 @@ public RobotVisualizer getRobotVisualizer() { } public void putShuffleboardCommands() { - SmartShuffleboard.putCommand("What Branch Unit Test", "Test 1", new UnitTester()); if (Constants.CORAL_DEBUG) { SmartDashboard.putData( "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java deleted file mode 100644 index 1ba6c7cc..00000000 --- a/src/main/java/frc/robot/commands/UnitTester.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utils.shuffleboard.SmartShuffleboard; - -public class UnitTester extends Command { - @Override - public void execute() { - SmartShuffleboard.put("Branch", "Branch", GamePieceLocate.UnitTest1().toString()); - } - - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 04692537..da30f85c 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -8,9 +8,9 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; -import frc.robot.commands.GamePieceLocate; import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.GamePieceLocate; import frc.robot.utils.diag.DiagLimelight; import java.util.Map; diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java similarity index 96% rename from src/main/java/frc/robot/commands/GamePieceLocate.java rename to src/main/java/frc/robot/utils/GamePieceLocate.java index 230fd1ee..2d3dc80e 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -1,4 +1,4 @@ -package frc.robot.commands; +package frc.robot.utils; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -181,10 +181,4 @@ public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) } return closest; } - - public static AlgaePositions UnitTest1() { - double x = 0.31119220563058896357; - double y = 0.06719517620178168871; - return findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); - } } diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java new file mode 100644 index 00000000..631037a4 --- /dev/null +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -0,0 +1,23 @@ +package frc.robot.utils; + +import static org.junit.jupiter.api.Assertions.*; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.constants.AlgaePositions; +import org.junit.jupiter.api.Test; + +class GamePieceLocateTest { + + @Test + void findAlgaePos() { + double x = 0.31119220563058896357; + double y = 0.06719517620178168871; + AlgaePositions algaePos = + GamePieceLocate.findAlgaePos( + new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); + assertEquals(5.144949195, algaePos.getPosition().getX()); + assertEquals(4.0259, algaePos.getPosition().getY()); + } +} From 035c70bf4cd1224727e811806e60f7c2e2ad91fb Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 24 Mar 2025 12:16:50 -0400 Subject: [PATCH 23/48] fixed noah's pr comments --- .../frc/robot/constants/GameConstants.java | 1 + .../subsystems/limelight/RealVisionIO.java | 24 +++++++++++++++---- .../subsystems/limelight/VisionInputs.java | 18 +++++--------- .../java/frc/robot/utils/GamePieceLocate.java | 4 ++-- 4 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index c599f96f..e3834e36 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -230,4 +230,5 @@ public enum Mode { new Transform3d( 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later + public static final double MINUMUM_PIECE_DETECTION_DOT = 0.8; } diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index da30f85c..9e4faa6e 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; +import frc.robot.constants.AlgaePositions; +import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.GamePieceLocate; @@ -45,12 +47,26 @@ public void updateInputs(VisionInputs inputs) { inputs.tx = tx.getDouble(0); inputs.ty = ty.getDouble(0); if (inputs.tv != 0) { - inputs.coralSeen.add( + BranchPositions[] newCoralArray = new BranchPositions[inputs.coralSeen.length + 1]; + AlgaePositions[] newAlgaeArray = new AlgaePositions[inputs.algaeSeen.length + 1]; + int oldCoralArrayLength = inputs.coralSeen.length; + int oldAlgaeArrayLength = inputs.algaeSeen.length; + for (int i = 0; i < oldCoralArrayLength; i++) { + newCoralArray[i] = inputs.coralSeen[i]; + } + newCoralArray[oldCoralArrayLength+1] = GamePieceLocate.findCoralBranch( - drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); - inputs.algaeSeen.add( + drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); + + for (int i = 0; i < oldAlgaeArrayLength; i++) { + newAlgaeArray[i] = inputs.algaeSeen[i]; + } + newAlgaeArray[oldAlgaeArrayLength+1] = GamePieceLocate.findAlgaePos( - drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); + drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); + + inputs.coralSeen = newCoralArray; + inputs.algaeSeen = newAlgaeArray; } } } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java index 908d6eec..13170f37 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -11,8 +11,8 @@ public class VisionInputs extends FolderLoggableInputs { public double tv = 0; public double tx = 0; public double ty = 0; - public ArrayList coralSeen; - public ArrayList algaeSeen; + public BranchPositions[] coralSeen; + public AlgaePositions[] algaeSeen; public VisionInputs(String folder) { super(folder); @@ -23,12 +23,8 @@ public void toLog(LogTable table) { table.put("tv", tv); table.put("tx", tx); table.put("ty", ty); - if (coralSeen != null) { - table.put("coralSeen", coralSeen.toArray(BranchPositions[]::new)); - } - if (algaeSeen != null) { - table.put("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)); - } + table.put("coralSeen", coralSeen); + table.put("algaeSeen", algaeSeen); } @Override @@ -36,9 +32,7 @@ public void fromLog(LogTable table) { tv = table.get("tv", tv); tx = table.get("tx", tx); ty = table.get("ty", ty); - coralSeen = - new ArrayList<>(List.of(table.get("coralSeen", coralSeen.toArray(BranchPositions[]::new)))); - algaeSeen = - new ArrayList<>(List.of(table.get("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)))); + coralSeen = table.get("coralSeen", coralSeen); + algaeSeen = table.get("algaeSeen", algaeSeen); } } diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 2d3dc80e..75019b89 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -34,8 +34,8 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); - double maxDot = -1.0; + VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))).unit(); + double maxDot = Constants.MINUMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); if (n == 0) { From e50b0e71a2b4c5b6fef64a02ca12ac2a31820351 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 24 Mar 2025 12:18:34 -0400 Subject: [PATCH 24/48] small fix and linter --- src/main/java/frc/robot/constants/GameConstants.java | 2 +- .../java/frc/robot/subsystems/limelight/RealVisionIO.java | 4 ++-- src/main/java/frc/robot/subsystems/limelight/Vision.java | 5 ++--- .../java/frc/robot/subsystems/limelight/VisionInputs.java | 2 -- src/main/java/frc/robot/utils/GamePieceLocate.java | 2 +- 5 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index e3834e36..7a71d507 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -230,5 +230,5 @@ public enum Mode { new Transform3d( 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later - public static final double MINUMUM_PIECE_DETECTION_DOT = 0.8; + public static final double MINIMUM_PIECE_DETECTION_DOT = 0.8; } diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 9e4faa6e..3dbe2504 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -54,14 +54,14 @@ public void updateInputs(VisionInputs inputs) { for (int i = 0; i < oldCoralArrayLength; i++) { newCoralArray[i] = inputs.coralSeen[i]; } - newCoralArray[oldCoralArrayLength+1] = + newCoralArray[oldCoralArrayLength + 1] = GamePieceLocate.findCoralBranch( drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); for (int i = 0; i < oldAlgaeArrayLength; i++) { newAlgaeArray[i] = inputs.algaeSeen[i]; } - newAlgaeArray[oldAlgaeArrayLength+1] = + newAlgaeArray[oldAlgaeArrayLength + 1] = GamePieceLocate.findAlgaePos( drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 786636db..494e4954 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -9,7 +9,6 @@ import frc.robot.constants.BranchPositions; import frc.robot.utils.logging.subsystem.LoggableSystem; import frc.robot.utils.shuffleboard.SmartShuffleboard; -import java.util.ArrayList; public class Vision extends SubsystemBase { LoggableSystem system; @@ -36,11 +35,11 @@ public boolean isPieceSeen() { return system.getInputs().tv != 0; } - public ArrayList getAllBranchPosition() { + public BranchPositions[] getAllBranchPosition() { return system.getInputs().coralSeen; } - public ArrayList getAllAlgaePosition() { + public AlgaePositions[] getAllAlgaePosition() { return system.getInputs().algaeSeen; } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java index 13170f37..7d141965 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -3,8 +3,6 @@ import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; import frc.robot.utils.logging.subsystem.FolderLoggableInputs; -import java.util.ArrayList; -import java.util.List; import org.littletonrobotics.junction.LogTable; public class VisionInputs extends FolderLoggableInputs { diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 75019b89..68aee175 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -35,7 +35,7 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))).unit(); - double maxDot = Constants.MINUMUM_PIECE_DETECTION_DOT; + double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); if (n == 0) { From 671981ad33b633ec29e9e1f324ff864b291f7066 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 24 Mar 2025 12:19:17 -0400 Subject: [PATCH 25/48] small fix and linter --- src/main/java/frc/robot/utils/GamePieceLocate.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 68aee175..ac2cf463 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -112,7 +112,7 @@ public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); - double maxDot = -1.0; + double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); if (n == 0) { From 2a1e4d19f65defc7ea9938e6988419505cf06ae2 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 08:51:58 -0400 Subject: [PATCH 26/48] fixed some pr --- .../java/frc/robot/utils/GamePieceLocate.java | 74 +++---------------- 1 file changed, 12 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index ac2cf463..410ea600 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -38,70 +38,20 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); - if (n == 0) { - for (int i = 0; i < 9; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } - } - for (int i = 33; i < 36; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } - } - } else if (n == 5) { - for (int i = 0; i < 3; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } - } - for (int i = 6 * n - 3; i < 6 * n + 6; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } - } - } else { - for (int i = 6 * n - 3; i < 6 * n + 9; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } + for (int i = 6 * n - 3; i < 6 * n + 9; i++) { + int f = i % BRANCHES.length; + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[f].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[f]; } } + return closest; } From edb20ed8dc33bde7e4e52f384ceb0b201972e6cb Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 09:08:30 -0400 Subject: [PATCH 27/48] fixed some pr --- src/main/java/frc/robot/constants/CenterPositions.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/CenterPositions.java b/src/main/java/frc/robot/constants/CenterPositions.java index fd6746f4..1b23e264 100644 --- a/src/main/java/frc/robot/constants/CenterPositions.java +++ b/src/main/java/frc/robot/constants/CenterPositions.java @@ -22,9 +22,9 @@ public Translation2d getPosition() { } public static int getClosest(Pose2d robotPose) { - double closeDistance = 100; + double closeDistance = robotPose.getTranslation().getDistance(values()[0].getPosition()); int n = 0; - for (int i = 0; i < values().length; i++) { + for (int i = 1; i < values().length; i++) { double distance = robotPose.getTranslation().getDistance(values()[i].getPosition()); if (distance < closeDistance) { n = i; From 6573d0731ae0f103fa2c65ddb37a1cf15f0191d1 Mon Sep 17 00:00:00 2001 From: Code Toad <55628278+goatfanboi23@users.noreply.github.com> Date: Tue, 25 Mar 2025 11:52:29 -0400 Subject: [PATCH 28/48] Nh limelight (#298) * fixed visionIO * use limelightHelper to get all vision measurements and not just the one with the most confidence * get multiple readings and calculate detection * fixed --------- Co-authored-by: codetoad --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../subsystems/limelight/RealVisionIO.java | 76 +- .../robot/subsystems/limelight/Vision.java | 55 +- .../subsystems/limelight/VisionInputs.java | 60 +- .../frc/robot/utils/LimelightHelpers.java | 1745 +++++++++++++++++ 5 files changed, 1872 insertions(+), 66 deletions(-) create mode 100644 src/main/java/frc/robot/utils/LimelightHelpers.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fac33ead..e786ea1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -164,7 +164,7 @@ public RobotContainer() { } } setupDriveTrain(); - vision = new Vision(new RealVisionIO(drivetrain)); + vision = new Vision(new RealVisionIO(), drivetrain::getPose); configureBindings(); setupAutoChooser(); putShuffleboardCommands(); diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 3dbe2504..6bacb1b6 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -1,35 +1,25 @@ package frc.robot.subsystems.limelight; import edu.wpi.first.cscore.HttpCamera; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; -import frc.robot.constants.AlgaePositions; -import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; -import frc.robot.utils.GamePieceLocate; +import frc.robot.utils.LimelightHelpers; import frc.robot.utils.diag.DiagLimelight; import java.util.Map; public class RealVisionIO implements VisionIO { - private final NetworkTableEntry tv; - private final NetworkTableEntry tx; - private final NetworkTableEntry ty; - private final SwerveDrivetrain drivetrain; + private LimelightHelpers.LimelightResults results; + private final NetworkTableEntry ledModeEntry; - public RealVisionIO(SwerveDrivetrain drivetrain) { - this.drivetrain = drivetrain; - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); - tv = table.getEntry("tv"); - tx = table.getEntry("tx"); - ty = table.getEntry("ty"); + public RealVisionIO() { Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); - + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + ledModeEntry = table.getEntry("ledMode"); HttpCamera limelightFeed = new HttpCamera( "limelight", "http://" + Constants.LIMELIGHT_IP_ADDRESS + ":5800/stream.mjpg"); @@ -43,30 +33,36 @@ public RealVisionIO(SwerveDrivetrain drivetrain) { @Override public void updateInputs(VisionInputs inputs) { - inputs.tv = tv.getDouble(0); - inputs.tx = tx.getDouble(0); - inputs.ty = ty.getDouble(0); - if (inputs.tv != 0) { - BranchPositions[] newCoralArray = new BranchPositions[inputs.coralSeen.length + 1]; - AlgaePositions[] newAlgaeArray = new AlgaePositions[inputs.algaeSeen.length + 1]; - int oldCoralArrayLength = inputs.coralSeen.length; - int oldAlgaeArrayLength = inputs.algaeSeen.length; - for (int i = 0; i < oldCoralArrayLength; i++) { - newCoralArray[i] = inputs.coralSeen[i]; - } - newCoralArray[oldCoralArrayLength + 1] = - GamePieceLocate.findCoralBranch( - drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); - - for (int i = 0; i < oldAlgaeArrayLength; i++) { - newAlgaeArray[i] = inputs.algaeSeen[i]; - } - newAlgaeArray[oldAlgaeArrayLength + 1] = - GamePieceLocate.findAlgaePos( - drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); - - inputs.coralSeen = newCoralArray; - inputs.algaeSeen = newAlgaeArray; + // this might take a while so we might now want to update this every tick and do it in another + // thread. + results = LimelightHelpers.getLatestResults(""); + LimelightHelpers.LimelightTarget_Detector[] targetsDetector = results.targets_Detector; + int detectionLength = targetsDetector.length; + inputs.className = new String[detectionLength]; + inputs.classID = new double[detectionLength]; + inputs.confidence = new double[detectionLength]; + inputs.ta = new double[detectionLength]; + inputs.tx = new double[detectionLength]; + inputs.ty = new double[detectionLength]; + inputs.tx_pixels = new double[detectionLength]; + inputs.ty_pixels = new double[detectionLength]; + for (int i = 0; i < detectionLength; i++) { + inputs.className[i] = targetsDetector[i].className; + inputs.classID[i] = targetsDetector[i].classID; + inputs.confidence[i] = targetsDetector[i].confidence; + inputs.ta[i] = targetsDetector[i].ta; + inputs.tx[i] = targetsDetector[i].tx; + inputs.ty[i] = targetsDetector[i].ty; + inputs.tx_pixels[i] = targetsDetector[i].tx_pixels; + inputs.ty_pixels[i] = targetsDetector[i].ty_pixels; } + inputs.pipelineID = results.pipelineID; + inputs.latency_pipeline = results.latency_pipeline; + inputs.latency_capture = results.latency_capture; + inputs.latency_jsonParse = results.latency_jsonParse; + inputs.timestamp_LIMELIGHT_publish = results.timestamp_LIMELIGHT_publish; + inputs.timestamp_RIOFPGA_capture = results.timestamp_RIOFPGA_capture; + inputs.valid = results.valid; + inputs.ledMode = (int) ledModeEntry.getInteger(-1); } } diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 494e4954..5bba6afc 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -4,53 +4,84 @@ package frc.robot.subsystems.limelight; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; +import frc.robot.utils.GamePieceLocate; import frc.robot.utils.logging.subsystem.LoggableSystem; -import frc.robot.utils.shuffleboard.SmartShuffleboard; +import java.util.ArrayList; +import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; public class Vision extends SubsystemBase { LoggableSystem system; + private final Supplier pose2dSupplier; + ArrayList currentAlgaePosition = new ArrayList<>(); + ArrayList currentCoralPositions = new ArrayList<>(); - public Vision(VisionIO io) { - system = new LoggableSystem<>(io, new VisionInputs("limelight")); + public Vision(VisionIO io, Supplier pose2dSupplier) { + this.system = new LoggableSystem<>(io, new VisionInputs("limelight")); + this.pose2dSupplier = pose2dSupplier; } /** * @return The piece's x offset angle in degrees and 0.0 if the piece isn't seen */ - public double getPieceOffsetAngleX() { + public double[] getPieceOffsetAngleX(int detectionIndex) { return system.getInputs().tx; } /** * @return The piece's y offset angle in degrees and 0.0 if the piece isn't seen */ - public double getPieceOffsetAngleY() { + public double[] getPieceOffsetAngleY() { return system.getInputs().ty; } public boolean isPieceSeen() { - return system.getInputs().tv != 0; + return system.getInputs().valid; } + @AutoLogOutput public BranchPositions[] getAllBranchPosition() { - return system.getInputs().coralSeen; + return currentCoralPositions.toArray(BranchPositions[]::new); } + @AutoLogOutput public AlgaePositions[] getAllAlgaePosition() { - return system.getInputs().algaeSeen; + return currentAlgaePosition.toArray(AlgaePositions[]::new); } @Override public void periodic() { system.updateInputs(); - if (getAllAlgaePosition() != null) { - SmartShuffleboard.put("peice pos", "Algae", getAllAlgaePosition()); + locateGamePieces(); + } + + private void locateGamePieces() { + currentAlgaePosition.clear(); + currentCoralPositions.clear(); + int detectionLength = system.getInputs().tx.length; + if (!system.getInputs().valid) { + return; } - if (getAllBranchPosition() != null) { - SmartShuffleboard.put("peice pos", "Coral", getAllBranchPosition()); + for (int i = 0; i < detectionLength; i++) { + String className = system.getInputs().className[i]; + if (className.equalsIgnoreCase("algae")) { + BranchPositions coralBranch = + GamePieceLocate.findCoralBranch( + pose2dSupplier.get(), + VecBuilder.fill(system.getInputs().tx[i], system.getInputs().ty[i])); + currentCoralPositions.add(coralBranch); + } else if (className.equalsIgnoreCase("coral")) { + AlgaePositions algaePos = + GamePieceLocate.findAlgaePos( + pose2dSupplier.get(), + VecBuilder.fill(system.getInputs().tx[i], system.getInputs().ty[i])); + currentAlgaePosition.add(algaePos); + } } } } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java index 7d141965..03ef64ff 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -1,16 +1,27 @@ package frc.robot.subsystems.limelight; -import frc.robot.constants.AlgaePositions; -import frc.robot.constants.BranchPositions; import frc.robot.utils.logging.subsystem.FolderLoggableInputs; import org.littletonrobotics.junction.LogTable; public class VisionInputs extends FolderLoggableInputs { - public double tv = 0; - public double tx = 0; - public double ty = 0; - public BranchPositions[] coralSeen; - public AlgaePositions[] algaeSeen; + public double pipelineID; + public double latency_pipeline; + public double latency_capture; + public double latency_jsonParse; + public double timestamp_LIMELIGHT_publish; + public double timestamp_RIOFPGA_capture; + public boolean valid; + public int ledMode = -1; + + // from LimelightHelpers.LimelightTarget_Detector[] + public String[] className = new String[0]; + public double[] classID = new double[0]; + public double[] confidence = new double[0]; + public double[] ta = new double[0]; + public double[] tx = new double[0]; + public double[] ty = new double[0]; + public double[] tx_pixels = new double[0]; + public double[] ty_pixels = new double[0]; public VisionInputs(String folder) { super(folder); @@ -18,19 +29,42 @@ public VisionInputs(String folder) { @Override public void toLog(LogTable table) { - table.put("tv", tv); + table.put("pipelineID", pipelineID); + table.put("latency_pipeline", latency_pipeline); + table.put("latency_capture", latency_capture); + table.put("latency_jsonParse", latency_jsonParse); + table.put("timestamp_LIMELIGHT_publish", timestamp_LIMELIGHT_publish); + table.put("timestamp_RIOFPGA_capture", timestamp_RIOFPGA_capture); + table.put("valid", valid); + table.put("classNames", className); + table.put("classID", classID); + table.put("confidence", confidence); + table.put("ta", ta); table.put("tx", tx); table.put("ty", ty); - table.put("coralSeen", coralSeen); - table.put("algaeSeen", algaeSeen); + table.put("tx_pixels", tx_pixels); + table.put("ty_pixels", ty_pixels); + table.put("ledMode", ledMode); } @Override public void fromLog(LogTable table) { - tv = table.get("tv", tv); + pipelineID = table.get("pipelineID", pipelineID); + latency_pipeline = table.get("latency_pipeline", latency_pipeline); + latency_capture = table.get("latency_capture", latency_capture); + latency_jsonParse = table.get("latency_jsonParse", latency_jsonParse); + timestamp_LIMELIGHT_publish = + table.get("timestamp_LIMELIGHT_publish", timestamp_LIMELIGHT_publish); + timestamp_RIOFPGA_capture = table.get("timestamp_RIOFPGA_capture", timestamp_RIOFPGA_capture); + valid = table.get("valid", valid); + className = table.get("classNames", className); + classID = table.get("classID", classID); + confidence = table.get("confidence", confidence); + ta = table.get("ta", ta); tx = table.get("tx", tx); ty = table.get("ty", ty); - coralSeen = table.get("coralSeen", coralSeen); - algaeSeen = table.get("algaeSeen", algaeSeen); + tx_pixels = table.get("tx_pixels", tx_pixels); + ty_pixels = table.get("ty_pixels", ty_pixels); + ledMode = table.get("ledMode", ledMode); } } diff --git a/src/main/java/frc/robot/utils/LimelightHelpers.java b/src/main/java/frc/robot/utils/LimelightHelpers.java new file mode 100644 index 00000000..b03f4d20 --- /dev/null +++ b/src/main/java/frc/robot/utils/LimelightHelpers.java @@ -0,0 +1,1745 @@ +// LimelightHelpers v1.12 (REQUIRES LLOS 2025.0 OR LATER) + +package frc.robot.utils; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +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.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Arrays; +import java.util.Map; +import java.util.concurrent.CompletableFuture; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision + * cameras in FRC. This library supports all Limelight features including AprilTag tracking, Neural + * Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** Represents a Color/Retroreflective Target Result extracted from JSON Output */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** Represents a Barcode Target Result extracted from JSON Output */ + public static class LimelightTarget_Barcode { + + /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */ + @JsonProperty("fam") + public String family; + + /** Gets the decoded data content of the barcode */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() {} + + public String getFamily() { + return family; + } + } + + /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() {} + } + + /** Represents a Neural Detector Pipeline Result extracted from JSON Output */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() {} + } + + /** Limelight Results object, parsed from a Limelight's JSON results output. */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + } + } + + /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + public RawFiducial( + int id, + double txnc, + double tync, + double ta, + double distToCamera, + double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + RawFiducial other = (RawFiducial) obj; + return id == other.id + && Double.compare(txnc, other.txnc) == 0 + && Double.compare(tync, other.tync) == 0 + && Double.compare(ta, other.ta) == 0 + && Double.compare(distToCamera, other.distToCamera) == 0 + && Double.compare(distToRobot, other.distToRobot) == 0 + && Double.compare(ambiguity, other.ambiguity) == 0; + } + } + + /** Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + public RawDetection( + int classId, + double txnc, + double tync, + double ta, + double corner0_X, + double corner0_Y, + double corner1_X, + double corner1_Y, + double corner2_X, + double corner2_Y, + double corner3_X, + double corner3_Y) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** Represents a 3D Pose Estimate. */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** Instantiates a PoseEstimate object with default values */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[] {}; + this.isMegaTag2 = false; + } + + public PoseEstimate( + Pose2d pose, + double timestampSeconds, + double latency, + int tagCount, + double tagSpan, + double avgTagDist, + double avgTagArea, + RawFiducial[] rawFiducials, + boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + PoseEstimate that = (PoseEstimate) obj; + // We don't compare the timestampSeconds as it isn't relevant for equality and makes + // unit testing harder + return Double.compare(that.latency, latency) == 0 + && tagCount == that.tagCount + && Double.compare(that.tagSpan, tagSpan) == 0 + && Double.compare(that.avgTagDist, avgTagDist) == 0 + && Double.compare(that.avgTagArea, avgTagArea) == 0 + && pose.equals(that.pose) + && Arrays.equals(rawFiducials, that.rawFiducials); + } + } + + /** Encapsulates the state of an internal Limelight IMU. */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + private static ObjectMapper mapper; + + /** Print JSON Parse time to the console in milliseconds */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if ("".equals(name) || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. Array format: [x, y, z, + * roll, pitch, yaw] where angles are in degrees. + * + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d( + Units.degreesToRadians(inData[3]), + Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. Uses only x, y, and yaw + * components, ignoring z, roll, and pitch. Array format: [x, y, z, roll, pitch, yaw] where angles + * are in degrees. + * + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. Note: z, roll, and + * pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate( + String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = + LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int) extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate( + pose, + adjustedTimestamp, + latency, + tagCount, + tagSpan, + tagDist, + tagArea, + rawFiducials, + isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = + new RawDetection( + classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, + corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. Includes timestamp, + * latency, tag count, tag span, average tag distance, average tag area, and detailed information + * about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent( + key, + k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + + ///// + ///// + + /** + * Does the Limelight have a valid target? + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the + * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable + * crosshair functionality. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the + * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable + * crosshair functionality. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, + * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier, + * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, + * targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector + * pipeline. + * + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass(String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass(String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field + * space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field + * space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling + * setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + } + + /** + * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch, + * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or + * unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow( + String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** Sets 3D offset point for easy 3D targeting. */ + public static void setFiducial3DOffset( + String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + SetRobotOrientation_INTERNAL( + limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + SetRobotOrientation_INTERNAL( + limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate, + boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if (flush) { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4) + * + * @param limelightName Name/identifier of the Limelight + * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the + * assist source more rapidly. + */ + public static void SetIMUAssistAlpha(String limelightName, double alpha) { + setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha); + } + + /** + * Configures the throttle value. Set to 100-200 while disabled to reduce thermal + * output/temperature. + * + * @param limelightName Name/identifier of the Limelight + * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping + * frames. + */ + public static void SetThrottle(String limelightName, int throttle) { + setLimelightNTDouble(limelightName, "throttle_set", throttle); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. Tags not in this list will + * be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. Increasing downscale can improve + * performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to + * 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) { + int d = 0; // pipeline + if (downscale == 1.0) { + d = 1; + } + if (downscale == 1.5) { + d = 2; + } + if (downscale == 2) { + d = 3; + } + if (downscale == 3) { + d = 4; + } + if (downscale == 4) { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace( + String limelightName, + double forward, + double side, + double up, + double roll, + double pitch, + double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** Asynchronously take snapshot. */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync( + () -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && !"".equals(snapshotName)) { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} From b41c8a72b5af2d6210bdc406cbd2e3254335d2d4 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:40:00 -0400 Subject: [PATCH 29/48] small pr fixed, changed from radians to degrees --- .../frc/robot/subsystems/limelight/Vision.java | 7 ++----- .../java/frc/robot/utils/GamePieceLocate.java | 17 ++++++++++++----- .../frc/robot/utils/GamePieceLocateTest.java | 4 +--- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 5bba6afc..8b3019d7 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -4,7 +4,6 @@ package frc.robot.subsystems.limelight; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AlgaePositions; @@ -72,14 +71,12 @@ private void locateGamePieces() { if (className.equalsIgnoreCase("algae")) { BranchPositions coralBranch = GamePieceLocate.findCoralBranch( - pose2dSupplier.get(), - VecBuilder.fill(system.getInputs().tx[i], system.getInputs().ty[i])); + pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); currentCoralPositions.add(coralBranch); } else if (className.equalsIgnoreCase("coral")) { AlgaePositions algaePos = GamePieceLocate.findAlgaePos( - pose2dSupplier.get(), - VecBuilder.fill(system.getInputs().tx[i], system.getInputs().ty[i])); + pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); currentAlgaePosition.add(algaePos); } } diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 410ea600..457a464e 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N3; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; @@ -29,12 +28,17 @@ public class GamePieceLocate { .toArray(Vector[]::new); // Piece Pos is in Radians - public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { + public static BranchPositions findCoralBranch( + 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(); final Vector pieceVec = - VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))).unit(); + VecBuilder.fill( + 1, + -Math.tan(piecePosTXDeg * Math.PI / 180), + -Math.tan(piecePosTYDeg * Math.PI / 180)) + .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); @@ -56,12 +60,15 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP } // piece pos is in Radians - public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) { + public static AlgaePositions findAlgaePos( + Pose2d robotPos, double piecePosXDeg, double piecePosYDeg) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); + VecBuilder.fill( + 1, -Math.tan(piecePosXDeg * Math.PI / 180), -Math.tan(piecePosYDeg * Math.PI / 180)) + .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index 631037a4..405e809c 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -2,7 +2,6 @@ import static org.junit.jupiter.api.Assertions.*; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.constants.AlgaePositions; @@ -15,8 +14,7 @@ void findAlgaePos() { double x = 0.31119220563058896357; double y = 0.06719517620178168871; AlgaePositions algaePos = - GamePieceLocate.findAlgaePos( - new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); + GamePieceLocate.findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), x, y); assertEquals(5.144949195, algaePos.getPosition().getX()); assertEquals(4.0259, algaePos.getPosition().getY()); } From 30003bc178c3290cea9a480d5237c9304a3004c0 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:44:23 -0400 Subject: [PATCH 30/48] small changes --- src/main/java/frc/robot/utils/GamePieceLocate.java | 4 +--- src/test/java/frc/robot/utils/GamePieceLocateTest.java | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 457a464e..a76e9762 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -35,9 +35,7 @@ public static BranchPositions findCoralBranch( final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill( - 1, - -Math.tan(piecePosTXDeg * Math.PI / 180), - -Math.tan(piecePosTYDeg * Math.PI / 180)) + 1, -Math.tan(Math.toRadians(piecePosTXDeg)), -Math.tan(Math.toRadians(piecePosTYDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index 405e809c..bd500fa0 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -8,7 +8,7 @@ import org.junit.jupiter.api.Test; class GamePieceLocateTest { - + // These are wrong I think @Test void findAlgaePos() { double x = 0.31119220563058896357; From ae91005e8748f1a62a11f4d2587865104e6bc7db Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:47:35 -0400 Subject: [PATCH 31/48] last --- src/main/java/frc/robot/utils/GamePieceLocate.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index a76e9762..bba4a672 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -27,7 +27,7 @@ public class GamePieceLocate { .map(algae -> algae.getPosition().getTranslation().toVector()) .toArray(Vector[]::new); - // Piece Pos is in Radians + // piece pos is in DEGREES, not RD public static BranchPositions findCoralBranch( Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); @@ -57,7 +57,7 @@ public static BranchPositions findCoralBranch( return closest; } - // piece pos is in Radians + // piece pos is in DEGREES, not RD public static AlgaePositions findAlgaePos( Pose2d robotPos, double piecePosXDeg, double piecePosYDeg) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); From d4660c5fab038076cf65b7afc675f5791536a1a5 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:49:03 -0400 Subject: [PATCH 32/48] last --- src/main/java/frc/robot/utils/GamePieceLocate.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index bba4a672..db39f013 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -59,14 +59,14 @@ public static BranchPositions findCoralBranch( // piece pos is in DEGREES, not RD public static AlgaePositions findAlgaePos( - Pose2d robotPos, double piecePosXDeg, double piecePosYDeg) { + 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(); final Vector pieceVec = - VecBuilder.fill( - 1, -Math.tan(piecePosXDeg * Math.PI / 180), -Math.tan(piecePosYDeg * Math.PI / 180)) - .unit(); + VecBuilder.fill( + 1, -Math.tan(Math.toRadians(piecePosTXDeg)), -Math.tan(Math.toRadians(piecePosTYDeg))) + .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); From e21394d25f33280613e281de8b2d126cd8d5a057 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:49:14 -0400 Subject: [PATCH 33/48] last --- src/main/java/frc/robot/utils/GamePieceLocate.java | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index db39f013..5a68ccf9 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -35,7 +35,9 @@ public static BranchPositions findCoralBranch( final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill( - 1, -Math.tan(Math.toRadians(piecePosTXDeg)), -Math.tan(Math.toRadians(piecePosTYDeg))) + 1, + -Math.tan(Math.toRadians(piecePosTXDeg)), + -Math.tan(Math.toRadians(piecePosTYDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; @@ -64,9 +66,11 @@ public static AlgaePositions findAlgaePos( final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill( - 1, -Math.tan(Math.toRadians(piecePosTXDeg)), -Math.tan(Math.toRadians(piecePosTYDeg))) - .unit(); + VecBuilder.fill( + 1, + -Math.tan(Math.toRadians(piecePosTXDeg)), + -Math.tan(Math.toRadians(piecePosTYDeg))) + .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); From 6d565ac684a221c9acc351e966831643c7df3390 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:56:34 -0400 Subject: [PATCH 34/48] fixed --- src/main/java/frc/robot/utils/GamePieceLocate.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 5a68ccf9..6e71aef3 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -36,8 +36,8 @@ public static BranchPositions findCoralBranch( final Vector pieceVec = VecBuilder.fill( 1, - -Math.tan(Math.toRadians(piecePosTXDeg)), - -Math.tan(Math.toRadians(piecePosTYDeg))) + -Math.tan(Math.toRadians(piecePosTYDeg)), + -Math.tan(Math.toRadians(piecePosTXDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; @@ -68,8 +68,8 @@ public static AlgaePositions findAlgaePos( final Vector pieceVec = VecBuilder.fill( 1, - -Math.tan(Math.toRadians(piecePosTXDeg)), - -Math.tan(Math.toRadians(piecePosTYDeg))) + -Math.tan(Math.toRadians(piecePosTYDeg)), + -Math.tan(Math.toRadians(piecePosTXDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; From bd584a29faf4b2aa7d3ccfb535c70848eca6fc65 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 16:00:55 -0400 Subject: [PATCH 35/48] done --- src/main/java/frc/robot/utils/GamePieceLocate.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 6e71aef3..12048e9c 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -36,8 +36,8 @@ public static BranchPositions findCoralBranch( final Vector pieceVec = VecBuilder.fill( 1, - -Math.tan(Math.toRadians(piecePosTYDeg)), - -Math.tan(Math.toRadians(piecePosTXDeg))) + -Math.tan(Math.toRadians(piecePosTXDeg)), + Math.tan(Math.toRadians(piecePosTYDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; @@ -68,8 +68,8 @@ public static AlgaePositions findAlgaePos( final Vector pieceVec = VecBuilder.fill( 1, - -Math.tan(Math.toRadians(piecePosTYDeg)), - -Math.tan(Math.toRadians(piecePosTXDeg))) + -Math.tan(Math.toRadians(piecePosTXDeg)), + Math.tan(Math.toRadians(piecePosTYDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; From 639624da4d61be92bb25ef963e2807b9ae04e0e5 Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Mar 2025 16:03:18 -0400 Subject: [PATCH 36/48] unit test --- .../java/frc/robot/constants/GameConstants.java | 5 ++--- .../java/frc/robot/utils/GamePieceLocateTest.java | 14 ++++++++------ 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 7a71d507..70d895f0 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -227,8 +227,7 @@ public enum Mode { // Limelight public static final Transform3d LIMELIGHT_TO_ROBOT = - new Transform3d( - 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later + new Transform3d(0.0, 0.0, 0, new Rotation3d(0.0, Math.PI / 6, 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.8; + public static final double MINIMUM_PIECE_DETECTION_DOT = 0; } diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index bd500fa0..c651683e 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -8,14 +8,16 @@ import org.junit.jupiter.api.Test; class GamePieceLocateTest { - // These are wrong I think + @Test void findAlgaePos() { - double x = 0.31119220563058896357; - double y = 0.06719517620178168871; + double robotX = Apriltag.EIGHTEEN.getX() - 0.889; + double robotY = Apriltag.EIGHTEEN.getY(); + double visionX = 5; // degrees + double visionY = -1; // degrees AlgaePositions algaePos = - GamePieceLocate.findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), x, y); - assertEquals(5.144949195, algaePos.getPosition().getX()); - assertEquals(4.0259, algaePos.getPosition().getY()); + GamePieceLocate.findAlgaePos( + new Pose2d(robotX, robotY, new Rotation2d(0)), visionX, visionY); + assertEquals(AlgaePositions.Algae_AB_LOW, algaePos); } } From 2e1a40f1394012577479b9b2e74f5a7961f72b7d Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 16:04:57 -0400 Subject: [PATCH 37/48] fixed algae to be more consistent --- .../java/frc/robot/utils/GamePieceLocate.java | 53 +------------------ 1 file changed, 1 insertion(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 12048e9c..f0cf2ab6 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -74,58 +74,8 @@ public static AlgaePositions findAlgaePos( double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); - if (n == 0) { - for (int i = 0; i < 6; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } - } - for (int i = 10; i < 12; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } - } - } else if (n == 5) { - for (int i = 0; i < 3; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } - } - for (int i = 8; i < 12; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } - } - } else { for (int i = 2 * n - 2; i < 2 * n + 4; i++) { + int f = i % ALGAES.length; Matrix locationVec = (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); double dot = @@ -136,7 +86,6 @@ public static AlgaePositions findAlgaePos( maxDot = dot; closest = ALGAES[i]; } - } } return closest; } From 2d20b22c45340b6c4c2dd004a49e86f76fb89a1f Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 16:05:38 -0400 Subject: [PATCH 38/48] fixed out of range --- .../java/frc/robot/utils/GamePieceLocate.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index f0cf2ab6..6a69f2f0 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -74,18 +74,18 @@ public static AlgaePositions findAlgaePos( double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); - for (int i = 2 * n - 2; i < 2 * n + 4; i++) { - int f = i % ALGAES.length; - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } + for (int i = 2 * n - 2; i < 2 * n + 4; i++) { + int f = i % ALGAES.length; + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[f].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[f]; + } } return closest; } From cec8d7945289f53c463e00d90f350f5ee62eaa5d Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Mar 2025 16:11:20 -0400 Subject: [PATCH 39/48] use Math.floorMod() because % does not work with negatives in java --- src/main/java/frc/robot/utils/GamePieceLocate.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 6a69f2f0..2eba35da 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -43,7 +43,7 @@ public static BranchPositions findCoralBranch( BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); for (int i = 6 * n - 3; i < 6 * n + 9; i++) { - int f = i % BRANCHES.length; + int f = Math.floorMod(i, BRANCHES.length); Matrix locationVec = (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[f].minus(cameraPosVec))); double dot = @@ -75,7 +75,7 @@ public static AlgaePositions findAlgaePos( AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); for (int i = 2 * n - 2; i < 2 * n + 4; i++) { - int f = i % ALGAES.length; + int f = Math.floorMod(i, ALGAES.length); Matrix locationVec = (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[f].minus(cameraPosVec))); double dot = From b06c20d34b846b457a8e7f450066da7572e0ed8c Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 10:09:31 -0400 Subject: [PATCH 40/48] omg it works!!!! --- src/main/java/frc/robot/constants/GameConstants.java | 2 +- src/test/java/frc/robot/utils/GamePieceLocateTest.java | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 70d895f0..7b5d58b2 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -227,7 +227,7 @@ public enum Mode { // Limelight public static final Transform3d LIMELIGHT_TO_ROBOT = - new Transform3d(0.0, 0.0, 0, new Rotation3d(0.0, Math.PI / 6, 0.0)); // z = 0.720725 + new Transform3d(0.0, 0.0, 0, new Rotation3d(0.0, -Math.PI / 6, 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; } diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index c651683e..8ce71917 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -13,11 +13,10 @@ class GamePieceLocateTest { void findAlgaePos() { double robotX = Apriltag.EIGHTEEN.getX() - 0.889; double robotY = Apriltag.EIGHTEEN.getY(); - double visionX = 5; // degrees - double visionY = -1; // degrees + double txDeg = 0; // degrees + double dyDeg = 5; // degrees AlgaePositions algaePos = - GamePieceLocate.findAlgaePos( - new Pose2d(robotX, robotY, new Rotation2d(0)), visionX, visionY); + GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); assertEquals(AlgaePositions.Algae_AB_LOW, algaePos); } } From c6f47d5a04aa0a99eb54b0cda1295ceacf911b8d Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 10:11:28 -0400 Subject: [PATCH 41/48] removed limelight feed --- .../frc/robot/subsystems/limelight/RealVisionIO.java | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 6bacb1b6..0aa11b50 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -20,15 +20,6 @@ public RealVisionIO() { Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); ledModeEntry = table.getEntry("ledMode"); - HttpCamera limelightFeed = - new HttpCamera( - "limelight", "http://" + Constants.LIMELIGHT_IP_ADDRESS + ":5800/stream.mjpg"); - ShuffleboardTab dashboardTab = Shuffleboard.getTab("Driver"); - dashboardTab - .add("Limelight feed", limelightFeed) - .withSize(6, 4) - .withPosition(2, 0) - .withProperties(Map.of("Show Crosshair", false, "Show Controls", false)); } @Override From f733205b3edbf6d02af3c11202357ab5dc077f48 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 10:13:48 -0400 Subject: [PATCH 42/48] added more tests --- .../subsystems/limelight/RealVisionIO.java | 5 ----- .../frc/robot/utils/GamePieceLocateTest.java | 20 ++++++++++++++++++- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 0aa11b50..214d4233 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -1,16 +1,11 @@ package frc.robot.subsystems.limelight; -import edu.wpi.first.cscore.HttpCamera; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; -import frc.robot.constants.Constants; import frc.robot.utils.LimelightHelpers; import frc.robot.utils.diag.DiagLimelight; -import java.util.Map; public class RealVisionIO implements VisionIO { private LimelightHelpers.LimelightResults results; diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index 8ce71917..f59effb6 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -14,9 +14,27 @@ void findAlgaePos() { double robotX = Apriltag.EIGHTEEN.getX() - 0.889; double robotY = Apriltag.EIGHTEEN.getY(); double txDeg = 0; // degrees - double dyDeg = 5; // degrees + double dyDeg = -5; // degrees AlgaePositions algaePos = GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); assertEquals(AlgaePositions.Algae_AB_LOW, algaePos); + + txDeg = 0; // degrees + dyDeg = 20; // degrees + algaePos = + GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); + assertEquals(AlgaePositions.Algae_AB_HIGH, algaePos); + + txDeg = 10; // degrees + dyDeg = 0; // degrees + algaePos = + GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); + assertEquals(AlgaePositions.Algae_CD_LOW, algaePos); + + txDeg = -10; // degrees + dyDeg = 0; // degrees + algaePos = + GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); + assertEquals(AlgaePositions.Algae_KL_LOW, algaePos); } } From e4f0ad57bbc1f37eb734e5282237137abd53b821 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 10:25:56 -0400 Subject: [PATCH 43/48] built code --- .../java/frc/robot/subsystems/limelight/RealVisionIO.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 214d4233..de07ec50 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -12,7 +12,9 @@ public class RealVisionIO implements VisionIO { private final NetworkTableEntry ledModeEntry; public RealVisionIO() { - Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); + Robot.getDiagnostics().addDiagnosable( + + new DiagLimelight("Vision", "Piece Seen")); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); ledModeEntry = table.getEntry("ledMode"); } From e3bc5f99bc187a5b707973dae88dae126b78675f Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 12:04:49 -0400 Subject: [PATCH 44/48] fixed some pr --- .../frc/robot/constants/GameConstants.java | 2 ++ .../robot/subsystems/limelight/Vision.java | 25 +++++++++++---- .../java/frc/robot/utils/GamePieceLocate.java | 31 ++++++++++++++----- 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 7b5d58b2..efc03c1c 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -230,4 +230,6 @@ public enum Mode { new Transform3d(0.0, 0.0, 0, new Rotation3d(0.0, -Math.PI / 6, 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 PIECE_DETECTION_PROBABILITY_SCALAR = 1.0; // TODO: change later + public static final double MINUMUM_PIECE_DETECTION_CONFIRMED_DOT = 0.8; } diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 8b3019d7..cca8a2c4 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -5,19 +5,24 @@ package frc.robot.subsystems.limelight; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; +import frc.robot.constants.Constants; import frc.robot.utils.GamePieceLocate; import frc.robot.utils.logging.subsystem.LoggableSystem; import java.util.ArrayList; +import java.util.LinkedList; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; public class Vision extends SubsystemBase { LoggableSystem system; private final Supplier pose2dSupplier; + double[] algaeConfidences = new double[AlgaePositions.values().length]; ArrayList currentAlgaePosition = new ArrayList<>(); + double[] coralConfidences = new double[BranchPositions.values().length]; ArrayList currentCoralPositions = new ArrayList<>(); public Vision(VisionIO io, Supplier pose2dSupplier) { @@ -69,15 +74,23 @@ private void locateGamePieces() { for (int i = 0; i < detectionLength; i++) { String className = system.getInputs().className[i]; if (className.equalsIgnoreCase("algae")) { - BranchPositions coralBranch = - GamePieceLocate.findCoralBranch( + double[] returnArray = GamePieceLocate.findCoralBranch( pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); - currentCoralPositions.add(coralBranch); - } else if (className.equalsIgnoreCase("coral")) { AlgaePositions algaePos = - GamePieceLocate.findAlgaePos( + AlgaePositions.values()[(int) returnArray[0]]; + coralConfidences[(int)returnArray[0]]+= returnArray[1]* Constants.PIECE_DETECTION_PROBABILITY_SCALAR; + if (coralConfidences[(int)returnArray[0]] > Constants.MINUMUM_PIECE_DETECTION_CONFIRMED_DOT) { + currentAlgaePosition.add(algaePos); + } + } else if (className.equalsIgnoreCase("coral")) { + double[] returnArray = GamePieceLocate.findCoralBranch( pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); - currentAlgaePosition.add(algaePos); + BranchPositions coralBranch = + BranchPositions.values()[(int) returnArray[0]]; + coralConfidences[(int)returnArray[0]]+= returnArray[1] * Constants.PIECE_DETECTION_PROBABILITY_SCALAR; + if (coralConfidences[(int)returnArray[0]] > 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..fa5d9814 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[2]; for (int i = 6 * n - 3; i < 6 * n + 9; i++) { int f = Math.floorMod(i, BRANCHES.length); Matrix locationVec = @@ -51,16 +53,21 @@ 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; + return returnArray; } // piece pos is in DEGREES, not RD - public static AlgaePositions findAlgaePos( + 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(); @@ -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[2]; for (int i = 2 * n - 2; i < 2 * n + 4; i++) { int f = Math.floorMod(i, ALGAES.length); Matrix locationVec = @@ -83,10 +93,15 @@ 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; + return returnArray; } } From e9c5c970a91355a193df6975bf85fe4bfdd36cc7 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 19:13:26 -0400 Subject: [PATCH 45/48] added some small stuff --- .../frc/robot/constants/GameConstants.java | 1 + .../robot/subsystems/limelight/Vision.java | 24 ++++++++++++++----- .../java/frc/robot/utils/GamePieceLocate.java | 6 +++-- 3 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index efc03c1c..f8eba255 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -232,4 +232,5 @@ public enum Mode { public static final double MINIMUM_PIECE_DETECTION_DOT = 0; public static final double PIECE_DETECTION_PROBABILITY_SCALAR = 1.0; // TODO: change later public static final double MINUMUM_PIECE_DETECTION_CONFIRMED_DOT = 0.8; + public static final double DECAY_CONSTANT = 2.3; // TODO: change later } diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index cca8a2c4..05761deb 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -4,8 +4,16 @@ package frc.robot.subsystems.limelight; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation3d; +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; @@ -15,14 +23,16 @@ import java.util.ArrayList; import java.util.LinkedList; import java.util.function.Supplier; + +import org.ejml.simple.SimpleMatrix; import org.littletonrobotics.junction.AutoLogOutput; public class Vision extends SubsystemBase { LoggableSystem system; private final Supplier pose2dSupplier; - double[] algaeConfidences = new double[AlgaePositions.values().length]; + Matrix algaeConfidences = new Matrix(Nat.N12(),Nat.N1(),new double[12]); ArrayList currentAlgaePosition = new ArrayList<>(); - double[] coralConfidences = new double[BranchPositions.values().length]; + Matrix coralConfidences = new Matrix(Nat.N6(),Nat.N6(),new double[36]); ArrayList currentCoralPositions = new ArrayList<>(); public Vision(VisionIO io, Supplier pose2dSupplier) { @@ -62,6 +72,8 @@ public AlgaePositions[] getAllAlgaePosition() { public void periodic() { system.updateInputs(); locateGamePieces(); + algaeConfidences.elementPower(Constants.DECAY_CONSTANT); + coralConfidences.elementPower(Constants.DECAY_CONSTANT); } private void locateGamePieces() { @@ -78,8 +90,8 @@ private void locateGamePieces() { pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); AlgaePositions algaePos = AlgaePositions.values()[(int) returnArray[0]]; - coralConfidences[(int)returnArray[0]]+= returnArray[1]* Constants.PIECE_DETECTION_PROBABILITY_SCALAR; - if (coralConfidences[(int)returnArray[0]] > Constants.MINUMUM_PIECE_DETECTION_CONFIRMED_DOT) { + 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("coral")) { @@ -87,8 +99,8 @@ private void locateGamePieces() { pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); BranchPositions coralBranch = BranchPositions.values()[(int) returnArray[0]]; - coralConfidences[(int)returnArray[0]]+= returnArray[1] * Constants.PIECE_DETECTION_PROBABILITY_SCALAR; - if (coralConfidences[(int)returnArray[0]] > Constants.MINUMUM_PIECE_DETECTION_CONFIRMED_DOT) { + 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 fa5d9814..a4015bdb 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -43,7 +43,7 @@ public static double[] findCoralBranch( int n = CenterPositions.getClosest(robotPos); int maxf = 0; double secondDot = Constants.MINIMUM_PIECE_DETECTION_DOT; - double[] returnArray = new double[2]; + 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 = @@ -63,6 +63,7 @@ public static double[] findCoralBranch( } returnArray[0] = maxf; returnArray[1] = maxDot-secondDot; + returnArray[2] = maxDot; return returnArray; } @@ -83,7 +84,7 @@ public static double[] findAlgaePos( int n = CenterPositions.getClosest(robotPos); int maxf = 0; double secondDot = Constants.MINIMUM_PIECE_DETECTION_DOT; - double[] returnArray = new double[2]; + 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 = @@ -102,6 +103,7 @@ public static double[] findAlgaePos( } returnArray[0] = maxf; returnArray[1] = maxDot-secondDot; + returnArray[2] = maxDot; return returnArray; } } From 81373b6d0fe2f27532dc1fa743a805e10a3448b3 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 20:14:21 -0400 Subject: [PATCH 46/48] added some "reasonable" estimates for what the constants should be --- src/main/java/frc/robot/constants/GameConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index f8eba255..94239a80 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -229,8 +229,8 @@ public enum Mode { public static final Transform3d LIMELIGHT_TO_ROBOT = new Transform3d(0.0, 0.0, 0, new Rotation3d(0.0, -Math.PI / 6, 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 PIECE_DETECTION_PROBABILITY_SCALAR = 1.0; // TODO: change later + 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 = 2.3; // TODO: change later + public static final double DECAY_CONSTANT = 1.0002; // TODO: change later } From 0564e2d4bc5661822fb0127725350b8577b6b7dd Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 21:44:25 -0400 Subject: [PATCH 47/48] linter --- .../subsystems/limelight/RealVisionIO.java | 3 +- .../robot/subsystems/limelight/Vision.java | 44 +++++++++++-------- .../java/frc/robot/utils/GamePieceLocate.java | 11 +++-- .../frc/robot/utils/GamePieceLocateTest.java | 8 ++-- 4 files changed, 35 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index de07ec50..134c691a 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -12,9 +12,8 @@ public class RealVisionIO implements VisionIO { private final NetworkTableEntry ledModeEntry; public RealVisionIO() { - Robot.getDiagnostics().addDiagnosable( + Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); - new DiagLimelight("Vision", "Piece Seen")); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); ledModeEntry = table.getEntry("ledMode"); } diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 05761deb..c7e3525b 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -6,13 +6,9 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation3d; 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; @@ -21,18 +17,15 @@ import frc.robot.utils.GamePieceLocate; import frc.robot.utils.logging.subsystem.LoggableSystem; import java.util.ArrayList; -import java.util.LinkedList; import java.util.function.Supplier; - -import org.ejml.simple.SimpleMatrix; import org.littletonrobotics.junction.AutoLogOutput; public class Vision extends SubsystemBase { LoggableSystem system; private final Supplier pose2dSupplier; - Matrix algaeConfidences = new Matrix(Nat.N12(),Nat.N1(),new double[12]); + 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]); + Matrix coralConfidences = new Matrix(Nat.N6(), Nat.N6(), new double[36]); ArrayList currentCoralPositions = new ArrayList<>(); public Vision(VisionIO io, Supplier pose2dSupplier) { @@ -86,21 +79,34 @@ private void locateGamePieces() { for (int i = 0; i < detectionLength; i++) { String className = system.getInputs().className[i]; if (className.equalsIgnoreCase("algae")) { - double[] returnArray = GamePieceLocate.findCoralBranch( + double[] returnArray = + GamePieceLocate.findCoralBranch( pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); - 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) { + 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("coral")) { - double[] returnArray = GamePieceLocate.findCoralBranch( + double[] returnArray = + GamePieceLocate.findCoralBranch( pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); - 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) { + 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 a4015bdb..209630c8 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -57,19 +57,18 @@ public static double[] findCoralBranch( maxDot = dot; maxf = f; } - if (dot>secondDot && dot!=maxDot){ + if (dot > secondDot && dot != maxDot) { secondDot = dot; } } returnArray[0] = maxf; - returnArray[1] = maxDot-secondDot; + returnArray[1] = maxDot - secondDot; returnArray[2] = maxDot; return returnArray; } // piece pos is in DEGREES, not RD - public static double[] 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(); @@ -97,12 +96,12 @@ public static double[] findAlgaePos( secondDot = maxDot; maxDot = dot; maxf = f; - } else if (dot>secondDot && dot!=maxDot){ + } else if (dot > secondDot && dot != maxDot) { secondDot = dot; } } returnArray[0] = maxf; - returnArray[1] = maxDot-secondDot; + 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..6490fc58 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -16,25 +16,25 @@ 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); } } From e727b6070464317cbe9d22b22b886ee312344143 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 21:50:56 -0400 Subject: [PATCH 48/48] fixed build and lint --- .../frc/robot/constants/GameConstants.java | 8 ++++---- .../robot/subsystems/limelight/Vision.java | 7 +++++-- .../frc/robot/utils/GamePieceLocateTest.java | 20 +++++++++++++++---- 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index aac0f89c..9aeaf8a2 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -236,9 +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.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 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 cd4f02cf..7ecad76f 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -7,6 +7,9 @@ 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; @@ -63,8 +66,8 @@ public void periodic() { locateGamePieces(); Logger.recordOutput("coralPoses", getAllBranchPosition()); Logger.recordOutput("algaePoses", getAllAlgaePosition()); - algaeConfidences.elementPower(Constants.DECAY_CONSTANT); - coralConfidences.elementPower(Constants.DECAY_CONSTANT); + algaeConfidences.elementPower(Constants.DECAY_CONSTANT); + coralConfidences.elementPower(Constants.DECAY_CONSTANT); } } diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index 6490fc58..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 = - AlgaePositions.values()[(int)GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]]; + 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 = - AlgaePositions.values()[(int)GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]]; + 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 = - AlgaePositions.values()[(int)GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]]; + 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 = - AlgaePositions.values()[(int)GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]]; + AlgaePositions.values()[ + (int) + GamePieceLocate.findAlgaePos( + new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg)[0]]; assertEquals(AlgaePositions.Algae_KL_LOW, algaePos); } }