Auto Preview Fixes

yay it works

src/main/deploy/pathplanner/autos/nothing.auto

@@ -0,0 +1,18 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.5,
+      "y": 2.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": []
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/java/frc/team2412/robot/util/AutonomousField.java

@@ -3,14 +3,13 @@
 import com.pathplanner.lib.path.PathPlannerTrajectory;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.Watchdog;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import frc.team2412.robot.util.auto.AutoLogic;
 import java.util.List;
 import java.util.Map;
 import java.util.Optional;
@@ -31,22 +30,23 @@ public static void configureShuffleboardTab(
 				tab.add("Auto display speed", DEFAULT_PLAYBACK_SPEED)
 						.withWidget(BuiltInWidgets.kNumberSlider)
 						.withProperties(Map.of("Min", 0.5, "Max", 2.5))
-						.withPosition(columnIndex, rowIndex + 2) // Offset by height of Field2d display
-						.withSize(2, 1)
+						.withPosition(columnIndex, rowIndex + 3) // Offset by height of Field2d display
+						.withSize(4, 1)
 						.getEntry();
-		StringSubscriber activeAutoSub =
-				NetworkTableInstance.getDefault()
-						.getTable("Shuffleboard/Match/Auto Chooser")
-						.getStringTopic("active")
-						.subscribe("");
+		// StringSubscriber activeAutoSub =
+		// 		NetworkTableInstance.getDefault()
+		// 				.getTable("Shuffleboard/Match/" + autoChooserName)
+		// 				.getStringTopic("active")
+		// 				.subscribe("");
 		var autonomousField =
 				new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED));
 		var watchdog =
 				new Watchdog(0.001, () -> DriverStation.reportWarning("auto field loop overrun", false));
 		addPeriodic.accept(
 				() -> {
 					watchdog.reset();
-					autonomousField.update(activeAutoSub.get());
+
+					autonomousField.update(AutoLogic.getSelectedAutoName());
 					watchdog.addEpoch("AutonomousField.update()");
 					watchdog.disable();
 					if (watchdog.isExpired()) {
@@ -56,7 +56,7 @@ public static void configureShuffleboardTab(
 				UPDATE_RATE);
 		tab.add("Selected auto", autonomousField.getField())
 				.withPosition(columnIndex, rowIndex)
-				.withSize(2, 2);
+				.withSize(4, 3);
 	}
 
 	// Display
@@ -109,6 +109,7 @@ public Pose2d getUpdatedPose(String autoName) {
 			lastName = Optional.of(autoName);
 			auto = PathPlannerAutos.getAuto(autoName);
 			trajectories = auto.trajectories;
+			System.out.println("num trajectories: " + trajectories.size());
 			trajectoryIndex = 0;
 			lastFPGATime = fpgaTime;
 			lastTrajectoryTimeOffset = 0;
@@ -123,6 +124,7 @@ public Pose2d getUpdatedPose(String autoName) {
 		lastFPGATime = fpgaTime;
 		while (lastTrajectoryTimeOffset > trajectories.get(trajectoryIndex).getTotalTimeSeconds()) {
 			lastTrajectoryTimeOffset -= trajectories.get(trajectoryIndex).getTotalTimeSeconds();
+			trajectoryIndex++;
 			if (trajectoryIndex >= trajectories.size()) {
 				trajectoryIndex = 0;
 			}
@@ -139,11 +141,14 @@ public Pose2d getUpdatedPose(String autoName) {
 	 *
 	 * @param autoName The name of the selected PathPlanner autonomous routine.
 	 */
-	public void update(String autoName) {
+	public void update(Optional<String> autoName) {
 		if (DriverStation.isEnabled()) {
 			lastName = Optional.empty();
 			return;
 		}
-		field.setRobotPose(getUpdatedPose(autoName));
+
+		if (autoName.isPresent()) {
+			field.setRobotPose(getUpdatedPose(autoName.get()));
+		}
 	}
 }

src/main/java/frc/team2412/robot/util/MatchDashboard.java

@@ -20,6 +20,6 @@ public MatchDashboard(Subsystems s) {
 		tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1);
 		tab.add(field).withPosition(0, 1).withSize(4, 3);
 		Robot r = Robot.getInstance();
-		AutonomousField.configureShuffleboardTab(tab, 6, 0, "Available Auto Variants", r::addPeriodic);
+		AutonomousField.configureShuffleboardTab(tab, 9, 0, "Available Auto Variants", r::addPeriodic);
 	}
 }

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

@@ -30,6 +30,7 @@
 import frc.team2412.robot.util.PathPlannerAutos;
 import java.util.List;
 import java.util.Map;
+import java.util.Optional;
 
 public class AutoLogic {
 	public static Robot r = Robot.getInstance();
@@ -46,17 +47,23 @@ public static enum StartPosition {
 				new Pose2d(0.73, 4.47, new Rotation2d(Units.degreesToRadians(120)))),
 		MISC("Misc", null);
 
-		String title; // for shuffleboard display
-		Pose2d startPose; // for identifying path's starting positions for filtering
+		final String title; // for shuffleboard display
+		final Pose2d startPose; // for identifying path's starting positions for filtering
 
 		StartPosition(String title, Pose2d startPose) {
 			this.title = title;
 			this.startPose = startPose;
 		}
 	};
 
+	static {
+		registerCommands();
+	}
+
 	// paths lists
 
+	private static AutoPath defaultPath = new AutoPath("do nothing", "nothing");
+
 	private static List<AutoPath> noPiecePaths =
 			List.of(
 					// presets
@@ -227,11 +234,11 @@ public static void initShuffleBoard() {
 			gameObjects.addOption(String.valueOf(i), i);
 		}
 
-		tab.add("Starting Position", startPositionChooser).withPosition(8, 0).withSize(2, 1);
-		tab.add("Launch Type", isVision).withPosition(8, 1);
-		tab.add("Game Objects", gameObjects).withPosition(9, 1);
-		tab.add("Available Auto Variants", availableAutos).withPosition(8, 2).withSize(2, 1);
-		autoDelayEntry = tab.add("Auto Delay", 0).withPosition(8, 3).withSize(1, 1).getEntry();
+		tab.add("Starting Position", startPositionChooser).withPosition(7, 0).withSize(2, 1);
+		tab.add("Launch Type", isVision).withPosition(7, 1);
+		tab.add("Game Objects", gameObjects).withPosition(8, 1);
+		tab.add("Available Auto Variants", availableAutos).withPosition(7, 2).withSize(2, 1);
+		autoDelayEntry = tab.add("Auto Delay", 0).withPosition(7, 3).withSize(1, 1).getEntry();
 
 		isVision.onChange(AutoLogic::filterAutos);
 		startPositionChooser.onChange(AutoLogic::filterAutos);
@@ -269,6 +276,17 @@ public static void filterAutos(Boolean isVision) {
 
 	// get auto
 
+	public static Optional<String> getSelectedAutoName() {
+		if (availableAutos.getSelected() == null) {
+			return Optional.empty();
+		}
+		return Optional.of(availableAutos.getSelected().getAutoName());
+	}
+
+	public static boolean chooserHasAutoSelected() {
+		return availableAutos.getSelected() != null;
+	}
+
 	public static Command getSelectedAuto() {
 		return Commands.sequence(
 				Commands.waitSeconds(autoDelayEntry.getDouble(0)),

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

@@ -1,22 +1,33 @@
 package frc.team2412.robot.util.auto;
 
+import java.util.ArrayList;
+import java.util.List;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPlannerTrajectory;
+
 import edu.wpi.first.math.MathUtil;
 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.wpilibj2.command.Command;
 import frc.team2412.robot.util.auto.AutoLogic.StartPosition;
 
 public class AutoPath {
 
 	private final Pose2d startPose2d;
 	private StartPosition startPosition;
+	private String pathPlannerAutoName;
 	private final String displayName;
 	private final Command autoCommand;
 	private final boolean vision;
 
+
 	public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) {
 		this.displayName = displayName;
+		this.pathPlannerAutoName = pathPlannerAutoName;
 		startPose2d = PathPlannerAuto.getStaringPoseFromAutoFile(pathPlannerAutoName);
 		autoCommand = AutoBuilder.buildAuto(pathPlannerAutoName);
 		this.vision = vision;
@@ -34,6 +45,7 @@ public AutoPath(String displayName, String pathPlannerAutoName, boolean vision)
 		}
 		// debug purposes
 		// System.out.println(startPosition + " " + displayName + " " + startPose2d.toString());
+
 	}
 
 	public AutoPath(String displayName, String pathPlannerAutoName) {
@@ -48,6 +60,10 @@ public Pose2d getStartPose2d() {
 		return startPose2d;
 	}
 
+	public String getAutoName() {
+		return pathPlannerAutoName;
+	}
+
 	public String getDisplayName() {
 		return displayName;
 	}
@@ -75,4 +91,6 @@ public boolean matchesStartPosition(StartPosition expectedStartPosition) {
 						startPose2d.getRotation().getDegrees(),
 						5));
 	}
+
+
 }