FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Limelight

This commit is contained in:
TheRealGrayden 2026-03-21 14:27:25 -04:00
parent 49399c7172
commit d2864098f3
1 changed files with 2 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java
View File

@ -3,29 +3,18 @@
// the WPILib BSD license file in the root directory of this project. // the WPILib BSD license file in the root directory of this project.
package org.fairportrobotics.frc.robolib.vision.limelight; package org.fairportrobotics.frc.robolib.vision.limelight;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
public class Limelight { public class Limelight {
private String name; private String name;
private Pigeon2 gyro;
private SwerveDrivePoseEstimator3d poseEstimator; private SwerveDrivePoseEstimator3d poseEstimator;
public Limelight(String name, Pigeon2 gyro, SwerveDrivePoseEstimator3d poseEstimator) { public Limelight(String name, SwerveDrivePoseEstimator3d poseEstimator) {
this.name = name; this.name = name;
this.gyro = gyro;
this.poseEstimator = poseEstimator; this.poseEstimator = poseEstimator;
} }
public LimelightHelpers.PoseEstimate updateOdometry() { public LimelightHelpers.PoseEstimate updateOdometry() {
boolean useMegaTag2 = true; //set to false to use MegaTag1
boolean doRejectUpdate = false;
LimelightHelpers.SetRobotOrientation(name, Math.toDegrees(poseEstimator.getEstimatedPosition().getRotation().getZ()), 0, 0, 0, 0, 0); LimelightHelpers.SetRobotOrientation(name, Math.toDegrees(poseEstimator.getEstimatedPosition().getRotation().getZ()), 0, 0, 0, 0, 0);
return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
} }