auto alignment

src/main/java/frc/team2412/robot/Robot.java

@@ -1,5 +1,8 @@
 package frc.team2412.robot;
 
+import static frc.team2412.robot.Subsystems.SubsystemConstants.APRILTAGS_ENABLED;
+import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED;
+
 import com.ctre.phoenix6.SignalLogger;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -21,6 +24,7 @@
 import frc.team2412.robot.commands.diagnostic.LauncherDiagnosticCommand;
 import frc.team2412.robot.util.MACAddress;
 import frc.team2412.robot.util.MatchDashboard;
+import frc.team2412.robot.util.auto.AutoAlignment;
 import frc.team2412.robot.util.auto.AutoLogic;
 
 public class Robot extends TimedRobot {
@@ -90,9 +94,11 @@ public void robotInit() {
 			AutoLogic.registerCommands();
 		}
 
-		if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) {
-			if (autoEnabled) {
-				AutoLogic.initShuffleBoard();
+		if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED && autoEnabled) {
+			AutoLogic.initShuffleboard();
+
+			if (APRILTAGS_ENABLED) {
+				AutoAlignment.initShuffleboard();
 			}
 		}
 
@@ -147,8 +153,8 @@ public void autonomousInit() {
 		// Checks if FMS is attatched and enables joystick warning if true
 		DriverStation.silenceJoystickConnectionWarning(!DriverStation.isFMSAttached());
 		// System.out.println(AutoLogic.getSelected() != null);
-		if (autoEnabled) {
-			if (AutoLogic.getSelectedAuto() != null && SubsystemConstants.DRIVEBASE_ENABLED) {
+		if (autoEnabled && SubsystemConstants.DRIVEBASE_ENABLED) {
+			if (AutoLogic.getSelectedAuto() != null) {
 				AutoLogic.getSelectedAuto().schedule();
 			}
 		}
@@ -188,6 +194,13 @@ public void disabledInit() {
 		coastCommand.schedule();
 	}
 
+	@Override
+	public void disabledPeriodic() {
+		if (DRIVEBASE_ENABLED && APRILTAGS_ENABLED && autoEnabled) {
+			AutoAlignment.updateField();
+		}
+	}
+
 	@Override
 	public void disabledExit() {
 		subsystems.drivebaseSubsystem.setMotorBrake(true);

src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java

@@ -0,0 +1,107 @@
+package frc.team2412.robot.util.auto;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.team2412.robot.Robot;
+import frc.team2412.robot.Subsystems;
+import java.util.function.Supplier;
+
+public class AutoAlignment {
+	private static Robot r = Robot.getInstance();
+	private static final Subsystems s = r.subsystems;
+
+	private static Supplier<Pose2d> currentPosition = s.drivebaseWrapper::getEstimatedPosition;
+	private static Supplier<Pose2d> goalPosition =
+			() -> AutoLogic.getSelectedAutoPath().getStartPose2d();
+
+	private static final double POSITION_TOLERANCE = 0.0254; // meters
+	private static final double ROTATION_TOLERANCE = 3; // degrees
+
+	private static Field2d field = new Field2d();
+	private static FieldObject2d robotPos;
+	private static FieldObject2d robotGoalPos;
+	private static ShuffleboardTab tab = Shuffleboard.getTab("AutoAlignment");
+
+	public static void initShuffleboard() {
+
+		tab.addString("Current Position", () -> getPose2dString(currentPosition.get()))
+				.withPosition(0, 0)
+				.withSize(3, 1);
+
+		tab.addString("Auto Starting Position", () -> getPose2dString(goalPosition.get()))
+				.withPosition(0, 1)
+				.withSize(3, 1);
+
+		tab.addBoolean("Correct Horizontal Position", AutoAlignment::isRobotInCorrectHorizontalPosition)
+				.withPosition(3, 0)
+				.withSize(2, 1);
+		tab.addBoolean("Correct Vertical Position", AutoAlignment::isRobotInCorrectVerticalPosition)
+				.withPosition(3, 1)
+				.withSize(2, 1);
+		tab.addBoolean("Correct Rotation", AutoAlignment::isRobotInCorrectRotation)
+				.withPosition(3, 2)
+				.withSize(2, 1);
+
+		tab.addBoolean("Correct Position", AutoAlignment::isRobotInCorrectPosition)
+				.withPosition(5, 0)
+				.withSize(2, 3);
+
+		initField();
+	}
+
+	private static boolean isRobotInCorrectPosition() {
+		return (isRobotInCorrectHorizontalPosition()
+				&& isRobotInCorrectVerticalPosition()
+				&& isRobotInCorrectRotation());
+	}
+
+	private static boolean isRobotInCorrectHorizontalPosition() {
+		return MathUtil.isNear(
+				currentPosition.get().getX(), goalPosition.get().getX(), POSITION_TOLERANCE);
+	}
+
+	private static boolean isRobotInCorrectVerticalPosition() {
+		return MathUtil.isNear(
+				currentPosition.get().getY(), goalPosition.get().getY(), POSITION_TOLERANCE);
+	}
+
+	private static boolean isRobotInCorrectRotation() {
+		return MathUtil.isNear(
+				currentPosition.get().getRotation().getDegrees(),
+				goalPosition.get().getRotation().getDegrees(),
+				ROTATION_TOLERANCE);
+	}
+
+	private static String getPose2dString(Pose2d pose) {
+		return "Translation: ("
+				+ pose.getX()
+				+ "m, "
+				+ pose.getY()
+				+ "m) | Rotation: ("
+				+ pose.getRotation().getDegrees()
+				+ " deg.)";
+	}
+
+	private static void initField() {
+		field = new Field2d();
+
+		SmartDashboard.putData("AlignmentField", field);
+		tab.add("Alignment Field", field).withPosition(7, 0).withSize(4, 3);
+
+		robotPos = field.getObject("Robot");
+		robotGoalPos = field.getObject("AutoStartPosition");
+
+		updateField();
+	}
+
+	public static void updateField() {
+		robotPos.setPose(s.drivebaseWrapper.getEstimatedPosition());
+
+		robotGoalPos.setPose(AutoLogic.getSelectedAutoPath().getStartPose2d());
+	}
+}

src/main/java/frc/team2412/robot/util/auto/AutoLogic.java

@@ -246,7 +246,7 @@ public static Command getAutoCommand(String pathName) {
 		return AutoBuilder.followPath(path);
 	}
 
-	public static void initShuffleBoard() {
+	public static void initShuffleboard() {
 
 		startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC);
 		for (StartPosition startPosition : StartPosition.values()) {
@@ -342,6 +342,9 @@ public static double getEstimatedAutoDuration() {
 		return 0;
 	}
 
+	public static AutoPath getSelectedAutoPath() {
+		return (chooserHasAutoSelected() ? availableAutos.getSelected() : defaultPath);
+	}
 	// commands util
 
 	public static BooleanSupplier isReadyToLaunch() {