Skip to content

Commit

Permalink
Limelight update to mega pose2
Browse files Browse the repository at this point in the history
  • Loading branch information
@frc1987
frc1987 committed Apr 14, 2024
1 parent bf0c034 commit 15dff20
Show file tree
Hide file tree
Showing 6 changed files with 151 additions and 26 deletions.
1 change: 1 addition & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ public void robotInit() {

@Override
public void robotPeriodic() {
robotContainer.DRIVETRAIN.updateOdometry();
robotContainer.updatePoseVision();
CommandScheduler.getInstance().run();
}
Expand Down
39 changes: 23 additions & 16 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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() ==
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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) {
Expand All @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/commands/movement/FastPoint.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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);
Expand All @@ -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
Expand Down
68 changes: 68 additions & 0 deletions src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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 */
Expand Down
57 changes: 53 additions & 4 deletions src/main/java/frc/robot/util/Limelight.java → ...java/frc/robot/util/LimelightHelpers.java
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// LimelightHelpers v1.4.0 (March 21, 2024)
// LimelightHelpers v1.5.0 (March 27, 2024)

package frc.robot.util;

Expand All @@ -24,7 +24,7 @@
import java.net.URL;
import java.util.concurrent.CompletableFuture;

public class Limelight {
public class LimelightHelpers {

public static class LimelightTarget_Retro {

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
*
Expand All @@ -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)
*
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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);
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/util/Util.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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()
Expand Down

0 comments on commit 15dff20

Please sign in to comment.