diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5ec05b7..1d5bc8a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -27,6 +27,7 @@ public void robotInit() { @Override public void robotPeriodic() { + robotContainer.DRIVETRAIN.updateOdometry(); robotContainer.updatePoseVision(); CommandScheduler.getInstance().run(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 19e4529..66b3a9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,7 +73,7 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Vision; import frc.robot.subsystems.Wrist; -import frc.robot.util.Limelight; +import frc.robot.util.LimelightHelpers; import frc.robot.util.Util; public class RobotContainer { @@ -149,15 +149,15 @@ private void configureDriverController() { .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2))) .andThen(new InstantCommand(() -> isReverseAmpPrimed = true)), () -> isReverseAmpPrimed)); - // DRIVER_CONTROLLER - // .back() - // .onTrue( - // DRIVETRAIN.runOnce( - // () -> { - // DRIVETRAIN.seedFieldRelative(); - // DRIVETRAIN.getPigeon2().reset(); - // })); - DRIVER_CONTROLLER.back().onTrue(new InstantCommand(() -> updatePoseVision(0.01, false))); + DRIVER_CONTROLLER + .back() + .onTrue( + DRIVETRAIN.runOnce( + () -> { + DRIVETRAIN.seedFieldRelative(); + DRIVETRAIN.getPigeon2().reset(); + }).andThen(new InstantCommand(() -> updatePoseVision(0.01, false)))); + // DRIVER_CONTROLLER.back().onTrue(new InstantCommand(() -> updatePoseVision(0.01, false))); DRIVER_CONTROLLER .start() .onTrue( @@ -459,6 +459,11 @@ public void configureShuffleboard() { // addAuto("GKC-Amp-J2"); AUTO_CHOOSER.addOption("Do Nothing", new InstantCommand()); MATCH_TAB.add("Auto", AUTO_CHOOSER); + MATCH_TAB.add("Full confidence", new InstantCommand(() -> DRIVETRAIN.addVisionMeasurement( + LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-leftlo").pose, + LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-leftlo").timestampSeconds, + VecBuilder.fill( + 0.1, 0.1, 0.1)))); // MATCH_TAB.addBoolean("Vision Updated", () -> VisionUpdate); // MATCH_TAB.addBoolean("is Alliance red", () -> CommandSwerveDrivetrain.getAlliance() == @@ -625,7 +630,7 @@ public Pose2d getPose() { return DRIVETRAIN.getPose(); } - public boolean shouldRejectLL3G(final Limelight.PoseEstimate botPose) { + public boolean shouldRejectLL3G(final LimelightHelpers.PoseEstimate botPose) { if (botPose.tagCount < 2) { return true; } @@ -662,11 +667,12 @@ public void updatePoseVision(double rotationConfidence, boolean canTrustLL3) { boolean isDisabled = DriverStation.isDisabled(); boolean hasUpdatedPose = false; - Limelight.PoseEstimate bestPose = null; + LimelightHelpers.PoseEstimate bestPose = null; double bestConfidence = 0.0; for (String limelight3G : Constants.Vision.LL3GS) { - Limelight.PoseEstimate botPoseLL3G = Limelight.getBotPoseEstimate_wpiBlue(limelight3G); + LimelightHelpers.PoseEstimate botPoseLL3G = + LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight3G); if (!shouldRejectLL3G(botPoseLL3G)) { double currentConfidence = calculateConfidence(botPoseLL3G); if (currentConfidence > bestConfidence) { @@ -686,7 +692,8 @@ public void updatePoseVision(double rotationConfidence, boolean canTrustLL3) { if (canTrustLL3) { if (!hasUpdatedPose) { for (String limelight3 : Constants.Vision.LL3S) { - Limelight.PoseEstimate botPoseLL3 = Limelight.getBotPoseEstimate_wpiBlue(limelight3); + LimelightHelpers.PoseEstimate botPoseLL3 = + LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight3); if (!shouldRejectLL3G(botPoseLL3)) { hasUpdatedPose = true; updatePose(botPoseLL3, isDisabled, rotationConfidence); @@ -704,7 +711,7 @@ public void updatePoseVision(double rotationConfidence, boolean canTrustLL3) { } public void updatePose( - Limelight.PoseEstimate botPose, boolean isDisabled, double rotationConfidence) { + LimelightHelpers.PoseEstimate botPose, boolean isDisabled, double rotationConfidence) { double currentConfidence = calculateConfidence(botPose); if (DriverStation.isAutonomous()) { currentConfidence = currentConfidence * 6; @@ -802,7 +809,7 @@ public void updatePose( // // } // } - private double calculateConfidence(Limelight.PoseEstimate botPose) { + private double calculateConfidence(LimelightHelpers.PoseEstimate botPose) { // final double botPoseToPoseDistance = // botPose.pose.getTranslation().getDistance(DRIVETRAIN.getPose().getTranslation()); final double speakerReference = Util.speakerTagCount(botPose.rawFiducials); diff --git a/src/main/java/frc/robot/commands/movement/FastPoint.java b/src/main/java/frc/robot/commands/movement/FastPoint.java index 77dd6ba..ed54400 100644 --- a/src/main/java/frc/robot/commands/movement/FastPoint.java +++ b/src/main/java/frc/robot/commands/movement/FastPoint.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.util.Limelight; +import frc.robot.util.LimelightHelpers; import frc.robot.util.Util; public class FastPoint extends Command { @@ -39,7 +39,7 @@ public FastPoint(CommandSwerveDrivetrain drivetrain, String limelightname) { @Override public void initialize() { - initialYaw = Limelight.getTX(limelightname); // TX or TY? + initialYaw = LimelightHelpers.getTX(limelightname); // TX or TY? initalRotation = drivetrain.getPose().getRotation().getDegrees(); thetaController.setSetpoint(initalRotation - initialYaw); @@ -66,7 +66,7 @@ public void execute() { @Override public boolean isFinished() { // return false; - return Util.isWithinTolerance(Limelight.getTX(limelightname), 0.0, 0.02); // TX or TY? + return Util.isWithinTolerance(LimelightHelpers.getTX(limelightname), 0.0, 0.02); // TX or TY? } @Override diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 4a40885..6b09107 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -14,9 +14,12 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; @@ -26,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.RobotContainer; import frc.robot.constants.DriveConstants; +import frc.robot.util.LimelightHelpers; import frc.robot.util.Util; import java.util.Optional; import java.util.function.Supplier; @@ -213,6 +217,70 @@ public static Alliance getAlliance() { return alliance; } + /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used + below are robot specific, and should be tuned. */ + private final SwerveDrivePoseEstimator m_poseEstimator = + new SwerveDrivePoseEstimator( + m_kinematics, + getPigeon2().getRotation2d(), + m_modulePositions, + new Pose2d(), + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); + + /** Updates the field relative position of the robot. */ + public void updateOdometry() { + m_poseEstimator.update(m_pigeon2.getRotation2d(), m_modulePositions); + + boolean useMegaTag2 = true; // set to false to use MegaTag1 + boolean doRejectUpdate = false; + if (useMegaTag2 == false) { + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-leftlo"); + + if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) { + if (mt1.rawFiducials[0].ambiguity > .7) { + doRejectUpdate = true; + } + if (mt1.rawFiducials[0].distToCamera > 3) { + doRejectUpdate = true; + } + } + if (mt1.tagCount == 0) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); + m_poseEstimator.addVisionMeasurement(mt1.pose, mt1.timestampSeconds); + } + } else if (useMegaTag2 == true) { + LimelightHelpers.SetRobotOrientation( + "limelight-leftlo", + this.getPose().getRotation().getDegrees(), + 0, + 0, + 0, + 0, + 0); + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-leftlo"); + if (Math.abs( + Units.radiansToDegrees(this.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond)) + > 720) // if our angular velocity is greater than 720 degrees per second, ignore vision + // updates + { + doRejectUpdate = true; + } + if (mt2.tagCount == 0) { + doRejectUpdate = true; + } + if (!doRejectUpdate) { + m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); + m_poseEstimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); + } + } + } + @Override public void periodic() { /* Periodically try to apply the operator perspective */ diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/LimelightHelpers.java similarity index 93% rename from src/main/java/frc/robot/util/Limelight.java rename to src/main/java/frc/robot/util/LimelightHelpers.java index b7e880e..c0ab3dd 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/LimelightHelpers.java @@ -1,4 +1,4 @@ -// LimelightHelpers v1.4.0 (March 21, 2024) +// LimelightHelpers v1.5.0 (March 27, 2024) package frc.robot.util; @@ -24,7 +24,7 @@ import java.net.URL; import java.util.concurrent.CompletableFuture; -public class Limelight { +public class LimelightHelpers { public static class LimelightTarget_Retro { @@ -472,7 +472,7 @@ private static double extractBotPoseEntry(double[] inData, int position) { } private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = Limelight.getLimelightNTTableEntry(limelightName, entryName); + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); var poseArray = poseEntry.getDoubleArray(new double[0]); var pose = toPose2D(poseArray); double latency = extractBotPoseEntry(poseArray, 6); @@ -752,6 +752,17 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpiblue"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the BLUE alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) * @@ -775,6 +786,17 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpired"); } + /** + * 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"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) * @@ -853,6 +875,33 @@ public static void setCropWindow( setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + + 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); + } + + 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); + } + public static void setCameraPose_RobotSpace( String limelightName, double forward, @@ -918,7 +967,7 @@ private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) public static LimelightResults getLatestResults(String limelightName) { long start = System.nanoTime(); - Limelight.LimelightResults results = new Limelight.LimelightResults(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); if (mapper == null) { mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java index 6221c93..3bdf429 100644 --- a/src/main/java/frc/robot/util/Util.java +++ b/src/main/java/frc/robot/util/Util.java @@ -24,7 +24,7 @@ import frc.robot.RobotContainer; import frc.robot.constants.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.util.Limelight.RawFiducial; +import frc.robot.util.LimelightHelpers.RawFiducial; import java.util.List; public class Util { @@ -153,13 +153,13 @@ public static boolean isWithinTolerance( } public static boolean canSeeTarget(String limelight) { - return Limelight.getBotPoseEstimate_wpiBlue(limelight).tagCount > 0; + return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight).tagCount > 0; } public static void setupUtil() {} public static double getDistance(String limelight) { - var result = Limelight.getBotPoseEstimate_wpiBlue(limelight); + var result = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight); if (result.tagCount > 0) { return new Pose3d(result.pose) .getTranslation()