LoggedAutoChooser.java

// Copyright (c) Choreo contributors

package frc.robot.autos;

import choreo.auto.AutoChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.littletonrobotics.junction.LogTable;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.inputs.LoggableInputs;
import org.littletonrobotics.junction.networktables.LoggedNetworkInput;

/**
 * Wrapper around Choreo's {@link AutoChooser} that logs the chosen auto routine with Advantagekit
 * for replay compatibility.
 */
public class LoggedAutoChooser extends AutoChooser {
  /**
   * Creates a new {@link LoggedAutoChooser}.
   *
   * @param key The key for the chooser, published to "/SmartDashboard/{key}" for NT or
   *     "/DashboardInputs/SmartDashboard/{key}" when logged.
   */
  public LoggedAutoChooser(String key) {
    this(key, "Nothing");
  }

  /**
   * Constructs a new {@link LoggedAutoChooser} with the given name for the do-nothing default
   * option.
   *
   * @param key The key for the chooser, published to "/SmartDashboard/{key}" for NT or
   *     "/DashboardInputs/SmartDashboard/{key}" when logged.
   * @param doNothingName The option name for the default choice.
   */
  public LoggedAutoChooser(String key, String doNothingName) {
    super(doNothingName);

    SmartDashboard.putData(key, this);

    LoggableInputs inputs =
        new LoggableInputs() {
          private String lastSelected = LoggedAutoChooser.this.getDefaultName();

          public void toLog(LogTable table) {
            this.lastSelected = LoggedAutoChooser.this.selectedCommand().getName();
            table.put(key, lastSelected);
          }

          public void fromLog(LogTable table) {
            lastSelected = table.get(key, lastSelected);
            LoggedAutoChooser.this.select(lastSelected);
          }
        };

    Logger.registerDashboardInput(
        new LoggedNetworkInput() {

          @Override
          public void periodic() {
            Logger.processInputs(prefix + "/SmartDashboard", inputs);
          }
        });
  }
}