Update auto chooser to use Trailblazer

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

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.arm.ArmSubsystem;
 import frc.robot.autos.Autos;
+import frc.robot.autos.trailblazer.Trailblazer;
 import frc.robot.config.RobotConfig;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.generated.BuildConstants;
@@ -52,7 +53,8 @@ public class Robot extends TimedRobot {
       new RobotManager(arm, shooter, localization, vision, imu, intake, queuer, swerve);
 
   private final RobotCommands robotCommands = new RobotCommands(robotManager);
-  private final Autos autos = new Autos(robotCommands, robotManager, swerve, localization);
+  private final Trailblazer trailblazer = new Trailblazer(swerve, localization);
+  private final Autos autos = new Autos(robotManager, trailblazer);
 
   private final LightsSubsystem lights = new LightsSubsystem(robotManager, hardware.candle);
 

src/main/java/frc/robot/autos/AutoChooser.java

@@ -1,83 +1,27 @@
 package frc.robot.autos;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import dev.doglog.DogLog;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.fms.FmsSubsystem;
+import frc.robot.autos.trailblazer.Trailblazer;
+import frc.robot.robot_manager.RobotManager;
 import java.util.EnumSet;
-import java.util.HashSet;
-import java.util.Optional;
-import java.util.Set;
 
 public class AutoChooser {
-  private final SendableChooser<AutoSelection> chooser = new SendableChooser<>();
-  private final Set<String> brokenAutoNames = new HashSet<>();
-  private final Command doNothingAuto;
-  private Optional<Command> cachedCommand = Optional.empty();
-  private String cachedAutoName = "";
+  private final SendableChooser<BaseAuto> chooser = new SendableChooser<>();
 
-  public AutoChooser(AutoCommands commands) {
+  public AutoChooser(RobotManager robotManager, Trailblazer trailblazer) {
     SmartDashboard.putData("Autos/SelectedAuto", chooser);
-    chooser.setDefaultOption(AutoSelection.DO_NOTHING.toString(), AutoSelection.DO_NOTHING);
 
     for (AutoSelection selection : EnumSet.allOf(AutoSelection.class)) {
-      chooser.addOption(selection.toString(), selection);
+      chooser.addOption(selection.toString(), selection.auto.apply(robotManager, trailblazer));
     }
 
-    this.doNothingAuto = commands.doNothingCommand();
+    chooser.setDefaultOption(
+        AutoSelection.DO_NOTHING.toString(),
+        AutoSelection.DO_NOTHING.auto.apply(robotManager, trailblazer));
   }
 
-  public Command getAutoCommand() {
-    var selection = getAutoSelection();
-
-    String autoName = FmsSubsystem.isRedAlliance() ? selection.redAutoName : selection.blueAutoName;
-
-    // When the name of the auto changes (either from changing alliance color or from making a new
-    // selection), clear out the cached command
-    if (!autoName.equals(cachedAutoName)) {
-      cachedCommand = Optional.empty();
-    }
-
-    cachedAutoName = autoName;
-
-    DogLog.log(
-        "Autos/BrokenAutoNames", brokenAutoNames.toArray(new String[brokenAutoNames.size()]));
-    DogLog.log("Autos/SelectedAutoName", selection);
-    DogLog.log("Autos/SelectedAutoFileName", autoName.equals("") ? "(none)" : autoName);
-
-    if (cachedCommand.isPresent()) {
-      return cachedCommand.get();
-    }
-
-    if (autoName.equals("")) {
-      var command = Commands.print("No auto path provided, running do nothing auto");
-      cachedCommand = Optional.of(command.andThen(doNothingAuto));
-      return command;
-    }
-
-    try {
-      var command =
-          AutoBuilder.buildAuto(autoName)
-              .withName("Auto__" + selection.toString() + "__" + autoName);
-
-      cachedCommand = Optional.of(command);
-      return command;
-    } catch (Exception e) {
-      var alliance = FmsSubsystem.isRedAlliance() ? "red" : "blue";
-      brokenAutoNames.add(selection.toString() + " - " + alliance + ": " + autoName);
-    }
-
-    var command = Commands.print("Auto failed to load, check Autos/BrokenAutoNames");
-    cachedCommand = Optional.of(command);
-    return command;
-  }
-
-  public AutoSelection getAutoSelection() {
-    var selected = Optional.ofNullable(chooser.getSelected());
-
-    return selected.orElse(AutoSelection.DO_NOTHING);
+  public BaseAuto getSelectedAuto() {
+    return chooser.getSelected();
   }
 }

src/main/java/frc/robot/autos/AutoSelection.java

@@ -1,19 +1,18 @@
 package frc.robot.autos;
 
+import frc.robot.autos.amp.Op345Auto;
+import frc.robot.autos.trailblazer.Trailblazer;
+import frc.robot.robot_manager.RobotManager;
+import java.util.function.BiFunction;
+
 public enum AutoSelection {
-  DO_NOTHING("", ""),
+  DO_NOTHING(DoNothingAuto::new),
 
-  AMP_5_PIECE("Red Amp 5 Piece", "Blue Amp 5 Piece"),
-  SOURCE_4_PIECE("Red Source 4 Piece", "Blue Source 4 Piece"),
-  SOURCE_OFFSET("Red Source Offset", "Blue Source Offset"),
-  MID_5_PIECE("Red 5 Piece 6 to 1", "Blue 5 Piece 6 to 1"),
-  OP("Red OP", "Blue OP");
+  OP(Op345Auto::new);
 
-  public final String redAutoName;
-  public final String blueAutoName;
+  public final BiFunction<RobotManager, Trailblazer, BaseAuto> auto;
 
-  private AutoSelection(String redAutoName, String blueAutoName) {
-    this.redAutoName = redAutoName;
-    this.blueAutoName = blueAutoName;
+  private AutoSelection(BiFunction<RobotManager, Trailblazer, BaseAuto> auto) {
+    this.auto = auto;
   }
 }

src/main/java/frc/robot/autos/Autos.java

@@ -1,90 +1,21 @@
 package frc.robot.autos;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
-import com.pathplanner.lib.util.PIDConstants;
-import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.localization.LocalizationSubsystem;
-import frc.robot.robot_manager.RobotCommands;
+import frc.robot.autos.trailblazer.Trailblazer;
 import frc.robot.robot_manager.RobotManager;
-import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 
 public class Autos extends LifecycleSubsystem {
-
-  private static Command wrapAutoEvent(String commandName, Command command) {
-    return Commands.sequence(
-            Commands.print("[COMMANDS] Starting auto event " + commandName),
-            command.deadlineWith(
-                Commands.waitSeconds(5)
-                    .andThen(
-                        Commands.print(
-                            "[COMMANDS] Auto event "
-                                + commandName
-                                + " has been running for 5+ seconds!"))),
-            Commands.print("[COMMANDS] Finished auto event " + commandName))
-        .handleInterrupt(() -> System.out.println("[COMMANDS] Cancelled auto event " + commandName))
-        .withName(commandName);
-  }
-
-  private static void registerCommand(String eventName, Command command) {
-    NamedCommands.registerCommand(eventName, wrapAutoEvent("Auto_" + eventName, command));
-  }
-
-  private final RobotCommands robotCommands;
   private final AutoChooser autoChooser;
-  private final AutoCommands autoCommands;
-  private final SwerveSubsystem swerve;
-  private final LocalizationSubsystem localization;
 
-  public Autos(
-      RobotCommands robotCommands,
-      RobotManager robotManager,
-      SwerveSubsystem swerve,
-      LocalizationSubsystem localization) {
+  public Autos(RobotManager robotManager, Trailblazer trailblazer) {
     super(SubsystemPriority.AUTOS);
 
-    this.robotCommands = robotCommands;
-    this.swerve = swerve;
-    this.localization = localization;
-
-    autoCommands = new AutoCommands(robotCommands, robotManager);
-
-    autoChooser = new AutoChooser(autoCommands);
-
-    AutoBuilder.configureHolonomic(
-        localization::getPose,
-        localization::resetPose,
-        swerve::getRobotRelativeSpeeds,
-        (robotRelativeSpeeds) -> {
-          swerve.setRobotRelativeAutoSpeeds(robotRelativeSpeeds);
-        },
-        new HolonomicPathFollowerConfig(
-            new PIDConstants(4.0, 0.0, 0.0),
-            new PIDConstants(2.5, 0.0, 0.0),
-            4.4,
-            0.387,
-            new ReplanningConfig(true, true)),
-        () -> false,
-        swerve);
-
-    registerCommand("speakerShot", autoCommands.speakerShotWithTimeout());
-    registerCommand("intakeAssist", robotCommands.intakeAssistCommand());
-    registerCommand("dynamic5Piece", autoCommands.dynamicAmp5PieceCommand());
-    registerCommand("waitingSpeakerCommand", robotCommands.waitSpeakerCommand());
+    autoChooser = new AutoChooser(robotManager, trailblazer);
   }
 
   public Command getAutoCommand() {
-    return autoChooser.getAutoCommand();
-  }
-
-  @Override
-  public void disabledPeriodic() {
-    // Constantly load the selected auto to avoid lag on auto init
-    getAutoCommand();
+    return autoChooser.getSelectedAuto().getAutoCommand();
   }
 }

src/main/java/frc/robot/autos/BaseAuto.java

@@ -1,7 +1,9 @@
 package frc.robot.autos;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.autos.trailblazer.Trailblazer;
+import frc.robot.fms.FmsSubsystem;
 import frc.robot.robot_manager.RobotCommands;
 import frc.robot.robot_manager.RobotManager;
 
@@ -18,5 +20,11 @@ protected BaseAuto(RobotManager robotManager, Trailblazer trailblazer) {
     this.autoCommands = new AutoCommands(actions, robotManager);
   }
 
-  public abstract Command getAutoCommand();
+  protected abstract Command getRedAutoCommand();
+
+  protected abstract Command getBlueAutoCommand();
+
+  public Command getAutoCommand() {
+    return Commands.either(getRedAutoCommand(), getBlueAutoCommand(), FmsSubsystem::isRedAlliance);
+  }
 }

src/main/java/frc/robot/autos/DoNothingAuto.java

@@ -0,0 +1,22 @@
+package frc.robot.autos;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.autos.trailblazer.Trailblazer;
+import frc.robot.robot_manager.RobotManager;
+
+public class DoNothingAuto extends BaseAuto {
+  public DoNothingAuto(RobotManager robotManager, Trailblazer trailblazer) {
+    super(robotManager, trailblazer);
+  }
+
+  @Override
+  protected Command getBlueAutoCommand() {
+    return Commands.none();
+  }
+
+  @Override
+  protected Command getRedAutoCommand() {
+    return Commands.none();
+  }
+}

src/main/java/frc/robot/autos/red/amp/RedOp345Auto.java → src/main/java/frc/robot/autos/amp/Op345Auto.java

@@ -1,4 +1,4 @@
-package frc.robot.autos.red.amp;
+package frc.robot.autos.amp;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -8,15 +8,20 @@
 import frc.robot.autos.trailblazer.AutoPoint;
 import frc.robot.autos.trailblazer.AutoSegment;
 import frc.robot.autos.trailblazer.Trailblazer;
-import frc.robot.robot_manager.RobotCommands;
 import frc.robot.robot_manager.RobotManager;
 
-public class RedOp345Auto extends BaseAuto {
-  public RedOp345Auto(RobotManager robotManager, Trailblazer trailblazer, RobotCommands actions) {
+public class Op345Auto extends BaseAuto {
+  public Op345Auto(RobotManager robotManager, Trailblazer trailblazer) {
     super(robotManager, trailblazer);
   }
 
-  public Command getAutoCommand() {
+  @Override
+  protected Command getBlueAutoCommand() {
+    return Commands.none();
+  }
+
+  @Override
+  protected Command getRedAutoCommand() {
     return Commands.sequence(
         Commands.print("example command on auto start"),
         trailblazer.followSegment(