feat: uses poseEstimator + vision to get pose

src/main/java/frc/robot/Constants.java

@@ -1,5 +1,6 @@
 package frc.robot;
 
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 
 public final class Constants {
@@ -32,4 +33,9 @@ public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new i
 			Speaker
 		}
 	}
+
+	public final static class DriveConstants {
+		//TODO: init pose estimator with actual values
+		public static final SwerveDrivePoseEstimator kPoseEstimator = new SwerveDrivePoseEstimator(null, null, null, null);
+	}
 }

src/main/java/frc/robot/commands/AlignToTagCommand.java

@@ -3,14 +3,21 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.TagAlignConstants;
 import frc.robot.Constants.TagAlignConstants.AlignPosition;
 
 public class AlignToTagCommand extends CommandBase {
 
-	final Subsystem m_subsystem; // TODO: change type to drive subsystem
+	final Subsystem m_subsystem; // TODO: change type to drive subsystem type
 	final Pose2d m_target;
 	final PIDController m_PIDxy;
 	final PIDController m_PIDomega;
@@ -43,7 +50,7 @@ public void initialize() {
 
 	@Override
 	public void execute() {
-		Pose2d current = new Pose2d(); // TODO: get pose from april tag
+		Pose2d current = poseFromAprilTag();
 
 		double xSpeed = MathUtil.clamp(m_PIDxy.calculate(current.getX(), m_target.getX()),
 				-0, 0); // TODO: change clamp parameters and units
@@ -75,9 +82,42 @@ public void end(boolean interrupted) {
 
 	@Override
 	public boolean isFinished() {
-		Pose2d current = new Pose2d(); // TODO: get pose from april tag
+		Pose2d current = poseFromAprilTag();
 		return Math.pow(current.getX() - m_target.getX(), 2) + Math.pow(current.getY() - m_target.getY(), 2) > Math
 				.pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity
 				m_consecCount >= TagAlignConstants.kConsectol; // Reached setpoint
 	}
+
+	/**
+	 * Gets pose from vision measurements from limelight
+	 * 
+	 * @return Robot Pose
+	 */
+	Pose2d poseFromAprilTag() { // TODO: move this to drive subsystem
+		NetworkTable tb = NetworkTableInstance.getDefault().getTable("limelight");
+		DoubleArrayEntry botpose = null; // index 0: x, 1: y, 2: z, 3: roll, 4: pitch, 5: yaw, 6: timestamp. all angles
+																			// are in degrees
+
+		try {
+			botpose = tb.getDoubleArrayTopic("botpose").getEntry(new double[0]);
+			double[] bp = botpose.getAtomic().value; // Not sure how threading works in WPIlib w/ command scheduler. If it is
+																								// thread safe then atomic wont be needed. There is no need to use the
+																								// atomic timestamp since the topic has the timestamp stored in it (7th
+																								// element)
+
+			if (botpose.exists()) {
+				Pose2d visionPose = new Pose2d(new Translation2d(bp[0], bp[1]), new Rotation2d(bp[5] * Math.PI / 180));
+				DriveConstants.kPoseEstimator.addVisionMeasurement(visionPose, Timer.getFPGATimestamp() - (bp[6] / 1000));
+			} else {
+				// TODO: log limelight failure
+			}
+
+		} finally {
+			if (botpose != null && botpose.isValid())
+				botpose.close(); // TODO: verify this logic: make sure it will never throw an exception,
+													// otherwise add another trycatch block
+		}
+
+		return DriveConstants.kPoseEstimator.getEstimatedPosition();
+	}
 }