diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java index 6ad1cf4..f0ac33a 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java @@ -3,29 +3,18 @@ // the WPILib BSD license file in the root directory of this project. 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.kinematics.SwerveModulePosition; public class Limelight { private String name; - private Pigeon2 gyro; private SwerveDrivePoseEstimator3d poseEstimator; - public Limelight(String name, Pigeon2 gyro, SwerveDrivePoseEstimator3d poseEstimator) { + public Limelight(String name, SwerveDrivePoseEstimator3d poseEstimator) { this.name = name; - this.gyro = gyro; this.poseEstimator = poseEstimator; } - public LimelightHelpers.PoseEstimate updateOdometry() { - boolean useMegaTag2 = true; //set to false to use MegaTag1 - boolean doRejectUpdate = false; - + public LimelightHelpers.PoseEstimate updateOdometry() { LimelightHelpers.SetRobotOrientation(name, Math.toDegrees(poseEstimator.getEstimatedPosition().getRotation().getZ()), 0, 0, 0, 0, 0); return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); }