Robot.java

package frc.robot;

import static frc.robot.subsystems.vision.VisionConstants.*;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.game.Field;
import frc.game.GameState;
import frc.lib.AllianceSelector;
import frc.lib.AutoOption;
import frc.lib.AutoSelector;
import frc.lib.CommandZorroController;
import frc.lib.ControllerSelector;
import frc.lib.ControllerSelector.ControllerType;
import frc.lib.ControllerSelector.DriverConfig;
import frc.lib.ControllerSelector.DriverController;
import frc.lib.ControllerSelector.OperatorConfig;
import frc.lib.ZorroController.Axis;
import frc.robot.Constants.DIOPorts;
import frc.robot.auto.B_LeftTrenchAuto;
import frc.robot.auto.B_LeftTrenchMoveFirstAuto;
import frc.robot.auto.B_RightTrenchAuto;
import frc.robot.auto.B_RightTrenchMoveFirstAuto;
import frc.robot.auto.R_LeftTrenchAuto;
import frc.robot.auto.R_LeftTrenchMoveFirstAuto;
import frc.robot.auto.R_RightTrenchAuto;
import frc.robot.auto.R_RightTrenchMoveFirstAuto;
import frc.robot.commands.DriveCommands;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.DriveConstants;
import frc.robot.subsystems.drive.GyroIO;
import frc.robot.subsystems.drive.GyroIOBoron;
import frc.robot.subsystems.drive.ModuleIO;
import frc.robot.subsystems.drive.ModuleIOSimWPI;
import frc.robot.subsystems.drive.ModuleIOTalonFX;
import frc.robot.subsystems.feeder.Feeder;
import frc.robot.subsystems.feeder.KickerIO;
import frc.robot.subsystems.feeder.KickerIOSimSpark;
import frc.robot.subsystems.feeder.KickerIOSpark;
import frc.robot.subsystems.feeder.SpindexerIO;
import frc.robot.subsystems.feeder.SpindexerIOSimSpark;
import frc.robot.subsystems.feeder.SpindexerIOSpark;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.intake.IntakeArmIO;
import frc.robot.subsystems.intake.IntakeArmIOReal;
import frc.robot.subsystems.intake.IntakeArmIOSim;
import frc.robot.subsystems.intake.IntakeConstants.RollerConstants;
import frc.robot.subsystems.intake.RollerIO;
import frc.robot.subsystems.intake.RollerIOSimTalonFX;
import frc.robot.subsystems.intake.RollerIOTalonFX;
import frc.robot.subsystems.launcher.FlywheelIO;
import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX;
import frc.robot.subsystems.launcher.FlywheelIOTalonFX;
import frc.robot.subsystems.launcher.HoodIO;
import frc.robot.subsystems.launcher.HoodIOSimSpark;
import frc.robot.subsystems.launcher.HoodIOSpark;
import frc.robot.subsystems.launcher.Launcher;
import frc.robot.subsystems.launcher.TurretIO;
import frc.robot.subsystems.launcher.TurretIOSimSpark;
import frc.robot.subsystems.launcher.TurretIOSpark;
import frc.robot.subsystems.leds.LEDController;
import frc.robot.subsystems.vision.Vision;
import frc.robot.subsystems.vision.VisionIO;
import frc.robot.subsystems.vision.VisionIOPhotonVision;
import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
import frc.robot.util.CanandgyroThread;
import frc.robot.util.SparkOdometryThread;
import frc.robot.util.VisionThread;
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;

/**
 * The VM is configured to automatically run this class, and to call the functions corresponding to
 * each mode, as described in the TimedRobot documentation. If you change the name of this class or
 * the package after creating this project, you must also update the build.gradle file in the
 * project.
 */
public class Robot extends LoggedRobot {
  public static final AllianceSelector allianceSelector =
      new AllianceSelector(DIOPorts.allianceColorSelector);
  public static final AutoSelector autoSelector =
      new AutoSelector(DIOPorts.autonomousModeSelector, allianceSelector::getAllianceColor);
  public static final Field2d field = new Field2d();

  public final PowerDistribution powerDistribution = new PowerDistribution(1, ModuleType.kRev);

  // Subsystems
  private Drive drive;
  private Vision vision;
  private Launcher launcher;
  private Feeder feeder;
  private Intake intake;
  // private Hopper hopper;
  private LEDController leds = LEDController.getInstance();

  public Robot() {
    // Record metadata
    Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
    Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
    Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
    Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
    Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
    switch (BuildConstants.DIRTY) {
      case 0:
        Logger.recordMetadata("GitDirty", "All changes committed");
        break;
      case 1:
        Logger.recordMetadata("GitDirty", "Uncomitted changes");
        break;
      default:
        Logger.recordMetadata("GitDirty", "Unknown");
        break;
    }

    // Set up data receivers & replay source
    switch (Constants.currentMode) {
      case REAL: // Running on a real robot
        // Log to a USB stick ("/U/logs")
        Logger.addDataReceiver(new WPILOGWriter());
        Logger.addDataReceiver(new NT4Publisher());

        // Instantiate hardware IO implementations
        drive =
            new Drive(
                new GyroIOBoron(),
                new ModuleIOTalonFX(DriveConstants.FrontLeft),
                new ModuleIOTalonFX(DriveConstants.FrontRight),
                new ModuleIOTalonFX(DriveConstants.BackLeft),
                new ModuleIOTalonFX(DriveConstants.BackRight));
        vision =
            new Vision(
                drive::addVisionMeasurement,
                drive::getPose,
                new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera),
                new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera),
                new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera),
                new VisionIOPhotonVision(cameraBackLeftName, robotToBackLeftCamera));
        launcher =
            new Launcher(
                drive::getPose,
                drive::getRobotRelativeChassisSpeeds,
                new TurretIOSpark(),
                new FlywheelIOTalonFX(),
                new HoodIOSpark());
        intake =
            new Intake(
                new RollerIOTalonFX(RollerConstants.upperRollerConfig),
                new RollerIOTalonFX(RollerConstants.lowerRollerConfig),
                new IntakeArmIOReal());
        // hopper = new Hopper(new HopperIOReal());
        feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark());
        SmartDashboard.putData(new Compressor(PneumaticsModuleType.REVPH));
        break;

      case SIM: // Running a physics simulator
        // Log to NT
        Logger.addDataReceiver(new NT4Publisher());

        // Instantiate physics sim IO implementations
        drive =
            new Drive(
                new GyroIO() {},
                new ModuleIOSimWPI(DriveConstants.FrontLeft),
                new ModuleIOSimWPI(DriveConstants.FrontRight),
                new ModuleIOSimWPI(DriveConstants.BackLeft),
                new ModuleIOSimWPI(DriveConstants.BackRight));
        vision =
            new Vision(
                drive::addVisionMeasurement,
                drive::getPose,
                new VisionIOPhotonVisionSim(
                    cameraFrontRightName, robotToFrontRightCamera, drive::getPose),
                new VisionIOPhotonVisionSim(
                    cameraFrontLeftName, robotToFrontLeftCamera, drive::getPose),
                new VisionIOPhotonVisionSim(
                    cameraBackRightName, robotToBackRightCamera, drive::getPose),
                new VisionIOPhotonVisionSim(
                    cameraBackLeftName, robotToBackLeftCamera, drive::getPose));
        launcher =
            new Launcher(
                drive::getPose,
                drive::getRobotRelativeChassisSpeeds,
                new TurretIOSimSpark(),
                new FlywheelIOSimTalonFX(),
                new HoodIOSimSpark());
        feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark());
        intake =
            new Intake(
                new RollerIOSimTalonFX(RollerConstants.upperRollerConfig),
                new RollerIOSimTalonFX(RollerConstants.lowerRollerConfig),
                new IntakeArmIOSim());
        // hopper = new Hopper(new HopperIOSim());
        break;

      case REPLAY: // Replaying a log
      default:
        // Set up replay source
        setUseTiming(false); // Run as fast as possible
        String logPath = LogFileUtil.findReplayLog();
        Logger.setReplaySource(new WPILOGReader(logPath));
        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));

        // Disable IO implementations
        drive =
            new Drive(
                new GyroIO() {},
                new ModuleIO() {},
                new ModuleIO() {},
                new ModuleIO() {},
                new ModuleIO() {});
        vision =
            new Vision(
                drive::addVisionMeasurement,
                drive::getPose,
                new VisionIO() {},
                new VisionIO() {},
                new VisionIO() {},
                new VisionIO() {});
        launcher =
            new Launcher(
                drive::getPose,
                drive::getRobotRelativeChassisSpeeds,
                new TurretIO() {},
                new FlywheelIO() {},
                new HoodIO() {});
        intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {});
        // hopper = new Hopper(new HopperIO() {});
        feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {});
        break;
    }

    // Start background threads (for non-blocking CAN/network reads)
    SparkOdometryThread.getInstance().start();
    VisionThread.getInstance().start();
    CanandgyroThread.getInstance().start();

    // Start AdvantageKit logger
    Logger.start();

    configureControlPanelBindings();
    configureAutoOptions();

    SmartDashboard.putData(
        "Align Encoders",
        new InstantCommand(() -> drive.zeroAbsoluteEncoders()).ignoringDisable(true));
    SmartDashboard.putData(CommandScheduler.getInstance());
    SmartDashboard.putData(drive);
    SmartDashboard.putData(vision);
    SmartDashboard.putData(launcher);
    SmartDashboard.putData(feeder);
    SmartDashboard.putData(intake);
    SmartDashboard.putData("Field", field);
    Field.plotRegions();

    feeder.setDefaultCommand(Commands.startEnd(feeder::stop, () -> {}, feeder).withName("Stop"));
    intake.setDefaultCommand(intake.getDefaultCommand());
    // hopper.setDefaultCommand(hopper.getDefaultCommand());
    launcher.setDefaultCommand(
        launcher
            .initializeHoodCommand()
            .andThen(
                new RunCommand(
                        () -> launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()),
                        launcher)
                    .withName("Aim at hub")));
  }

  /** This function is called periodically during all modes. */
  @Override
  public void robotPeriodic() {
    long loopStart = Constants.PROFILING_ENABLED ? System.nanoTime() : 0;

    // Runs the Scheduler. This is responsible for polling buttons, adding
    // newly-scheduled commands, running already-scheduled commands, removing
    // finished or interrupted commands, and running subsystem periodic() methods.
    // This must be called from the robot's periodic block in order for anything in
    // the Command-based framework to work.
    CommandScheduler.getInstance().run();
    long t1 = Constants.PROFILING_ENABLED ? System.nanoTime() : 0;

    GameState.logValues();
    long t2 = Constants.PROFILING_ENABLED ? System.nanoTime() : 0;

    // Profiling output
    if (Constants.PROFILING_ENABLED) {
      Logger.recordOutput("Profiling/Robot/SchedulerMs", (t1 - loopStart) / 1_000_000);
      Logger.recordOutput("Profiling/Robot/GameStateMs", (t2 - t1) / 1_000_000);
      Logger.recordOutput("Profiling/Robot/TotalMs", (t2 - loopStart) / 1_000_000);
    }

    // Return to non-RT thread priority (do not modify the first argument)
    // Threads.setCurrentThreadPriority(false, 10);
  }

  /** This function is called once when the robot is disabled. */
  @Override
  public void disabledInit() {
    leds.clear();
  }

  /** This function is called periodically when disabled. */
  @Override
  public void disabledPeriodic() {
    allianceSelector.disabledPeriodic();
    autoSelector.disabledPeriodic();
    ControllerSelector.getInstance().scan(false);
    leds.displayAutoSelection();
    var autoOption = autoSelector.get();
    autoOption.ifPresent(
        a ->
            a.getInitialPose()
                .ifPresent(targetPose -> leds.displayPoseSeek(drive.getPose(), targetPose)));
  }

  /** This function is called once when autonomous mode is enabled. */
  @Override
  public void autonomousInit() {
    drive.setDefaultCommand(Commands.runOnce(drive::stop, drive).withName("Stop"));
    autoSelector.scheduleAuto();
    leds.clear();
  }

  /** This function is called periodically during autonomous. */
  @Override
  public void autonomousPeriodic() {
    leds.displayHubCountdown();
    leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning());
  }

  /** This function is called once when teleop mode is enabled. */
  @Override
  public void teleopInit() {
    autoSelector.cancelAuto();
    ControllerSelector.getInstance().scan(true);
    leds.clear();
  }

  /** This function is called periodically during operator control. */
  @Override
  public void teleopPeriodic() {
    leds.displayHubCountdown();
    leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning());
  }

  /** This function is called once when test mode is enabled. */
  @Override
  public void testInit() {
    // Cancels all running commands at the start of test mode.
    CommandScheduler.getInstance().cancelAll();
    leds.clear();
  }

  /** This function is called periodically during test mode. */
  @Override
  public void testPeriodic() {
    leds.displayHubCountdown();
    leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning());
  }

  /** This function is called once when the robot is first started up. */
  @Override
  public void simulationInit() {
    leds.clear();
  }

  /** This function is called periodically whilst in simulation. */
  @Override
  public void simulationPeriodic() {
    leds.displayHubCountdown();
    leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning());
  }

  private void configureControlPanelBindings() {
    ControllerSelector.configure(
        // ZORRO is always preferred as driver in REAL and SIM mode
        new DriverConfig(
            ControllerType.ZORRO, this::bindZorroDriver, Constants.Mode.REAL, Constants.Mode.SIM),
        // XBOX is always preferred as operator in REAL and SIM mode
        new OperatorConfig(
            ControllerType.XBOX, this::bindXboxOperator, Constants.Mode.REAL, Constants.Mode.SIM),
        // XBOX is permitted as driver in REAL and SIM mode
        new DriverConfig(
            ControllerType.XBOX, this::bindXboxDriver, Constants.Mode.REAL, Constants.Mode.SIM));
  }

  public DriverController bindZorroDriver(int port) {
    var zorroDriver = new CommandZorroController(port);

    var controller =
        new DriverController() {
          public double getXTranslationInput() {
            return -zorroDriver.getRightYAxis();
          }

          public double getYTranslationInput() {
            return -zorroDriver.getRightXAxis();
          }

          public double getRotationInput() {
            return -zorroDriver.getLeftXAxis();
          }

          public boolean getFieldRelativeInput() {
            return zorroDriver.getHID().getEUp();
          }
        };

    // Drive in field-relative mode while switch E is up
    // Drive in robot-relative mode while switch E is down
    drive.setDefaultCommand(
        DriveCommands.joystickDrive(
            drive,
            () -> controller.getXTranslationInput(),
            () -> controller.getYTranslationInput(),
            () -> controller.getRotationInput(),
            () -> controller.getFieldRelativeInput(),
            allianceSelector::fieldRotated));

    // Reset gyro to 0° when button G is pressed
    zorroDriver
        .GIn()
        .onTrue(
            Commands.runOnce(
                    () ->
                        drive.resetHeading(
                            allianceSelector.fieldRotated()
                                ? Rotation2d.k180deg
                                : Rotation2d.kZero),
                    drive)
                .ignoringDisable(true));

    // Switch to X pattern when button D is pressed
    // zorroDriver.DIn().onTrue(Commands.runOnce(drive::stopWithX, drive));

    // Desaturate turret and advance feeder
    zorroDriver.AIn().whileTrue(createDesaturateAndShootCommand(controller));

    // Launcher
    Trigger launcherEnabled = zorroDriver.axisGreaterThan(Axis.kLeftDial.value, 0.5).debounce(0.1);
    launcherEnabled
        .or(() -> DriverStation.isFMSAttached())
        .whileTrue(
            launcher
                .initializeHoodCommand()
                .andThen(
                    new RunCommand(
                            () ->
                                launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()),
                            launcher)
                        .withName("Aim at hub")));

    // Intake
    zorroDriver.HIn().whileTrue(intake.getDeployCommand());
    // zorroDriver.HIn().and(() -> hopper.isDeployed()).onTrue(intake.getDeployCommand());
    // zorroDriver.HIn().negate().and(() -> hopper.isDeployed()).onTrue(intake.getDefaultCommand());

    // Hopper
    // zorroDriver
    //     .FDown()
    //     .onTrue(Commands.startEnd(hopper::deploy, () -> {}, hopper).withName("Deploy"));

    // zorroDriver
    //     .FUp()
    //     .onTrue(
    //         new ConditionalCommand(
    //             // If intake is stowed, immediately retract hopper
    //             hopper.getDefaultCommand(),
    //             // If intake is deployed, retract it first before retracting hopper
    //             Commands.parallel(
    //                 intake.getDefaultCommand(),
    //
    // hopper.idle().withTimeout(Seconds.of(2)).andThen(hopper.getDefaultCommand())),
    //             // Condition to check
    //             () -> intake.isStowed()));

    return controller;
  }

  public DriverController bindXboxDriver(int port) {
    var xboxDriver = new CommandXboxController(port);

    var controller =
        new DriverController() {
          public double getXTranslationInput() {
            return -xboxDriver.getLeftY();
          }

          public double getYTranslationInput() {
            return -xboxDriver.getLeftX();
          }

          public double getRotationInput() {
            return -xboxDriver.getRightX();
          }

          public boolean getFieldRelativeInput() {
            return !xboxDriver.getHID().getLeftBumperButton();
          }
        };

    // Drive in field-relative mode while left bumper is released
    // Drive in robot-relative mode while left bumper is pressed
    drive.setDefaultCommand(
        DriveCommands.joystickDrive(
            drive,
            () -> controller.getXTranslationInput(),
            () -> controller.getYTranslationInput(),
            () -> controller.getRotationInput(),
            () -> controller.getFieldRelativeInput(),
            allianceSelector::fieldRotated));

    // Reset gyro to 0° when B button is pressed
    xboxDriver
        .b()
        .onTrue(
            Commands.runOnce(
                    () ->
                        drive.resetHeading(
                            allianceSelector.fieldRotated()
                                ? Rotation2d.k180deg
                                : Rotation2d.kZero),
                    drive)
                .ignoringDisable(true));

    // xboxDriver
    //     .a()
    //     .whileTrue(
    //         Commands.run(() -> launcher.aim(GameState.getMyHubPose().getTranslation()), launcher)
    //             .withName("Aim at hub"));

    // xboxDriver
    //     .y()
    //     .whileTrue(
    //         Commands.run(() -> launcher.aim(GameState.getFieldTarget().getTranslation()),
    // launcher)
    //             .withName("Aim at Target"));

    // // Point at Hub while A button is held
    // xboxDriver
    //     .a()
    //     .whileTrue(
    //         DriveCommands.joystickDriveAtFixedOrientation(
    //             drive,
    //             () -> -xboxDriver.getLeftY(),
    //             () -> -xboxDriver.getLeftX(),
    //             () ->
    //                 GameState.getMyHubPose()
    //                     .toPose2d()
    //                     .getTranslation()
    //                     .minus(drive.getPose().getTranslation())
    //                     .getAngle(),
    //             allianceSelector::fieldRotated));

    // // Point in the direction of the commanded translation while Y button is held
    // xboxDriver
    //     .y()
    //     .whileTrue(
    //         DriveCommands.joystickDrivePointedForward(
    //             drive,
    //             () -> -xboxDriver.getLeftY(),
    //             () -> -xboxDriver.getLeftX(),
    //             allianceSelector::fieldRotated));

    // Point at vision target while A button is held
    // xboxDriver
    //     .a()
    //     .whileTrue(
    //         DriveCommands.pointAtTarget(
    //             drive, () -> vision.getTargetX(0), allianceSelector::fieldRotated));

    // Drive 1m forward while A button is held
    // xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1)));

    // Align with pose, approaching in correct orientation from 1 m away
    // xboxDriver
    //     .a()
    //     .whileTrue(
    //         PathCommands.dockToTargetPose(
    //             drive, new Pose2d(8.2296, 4.1148, Rotation2d.kZero), Meters.of(1)));

    // Drive to point, approaching in correct orientation from 2 m away
    // xboxDriver
    //  .a()
    // .whileTrue(
    //     PathCommands.dockToTargetPoint(drive, new Translation2d(8.2296, 4.1148), Meters.of(2)));

    // Switch to X pattern when X button is pressed
    xboxDriver.x().onTrue(Commands.runOnce(drive::stopWithX, drive));

    return controller;
  }

  public void bindXboxOperator(int port, DriverController driver) {
    var xboxOperator = new CommandXboxController(port);

    // Intake
    xboxOperator.b().whileTrue(intake.getDeployCommand());
    // xboxOperator.b().and(() -> hopper.isDeployed()).whileTrue(intake.getDeployCommand());

    xboxOperator.y().whileTrue(intake.getReverseCommand());
    // xboxOperator.y().and(() -> hopper.isDeployed()).whileTrue(intake.getReverseCommand());

    // Feeder
    xboxOperator
        .a()
        .whileTrue(Commands.startEnd(feeder::spinForward, () -> {}, feeder).withName("Advance"));

    xboxOperator
        .x()
        .whileTrue(Commands.startEnd(feeder::reverse, () -> {}, feeder).withName("Reverse"));

    // Desaturate turret and advance feeder
    xboxOperator.rightBumper().whileTrue(createDesaturateAndShootCommand(driver));
  }

  public void configureAutoOptions() {
    autoSelector.addAuto(
        new AutoOption(Alliance.Blue, 1, new B_LeftTrenchAuto(drive, feeder, intake, launcher)));
    autoSelector.addAuto(
        new AutoOption(Alliance.Red, 1, new R_LeftTrenchAuto(drive, feeder, intake, launcher)));
    autoSelector.addAuto(
        new AutoOption(Alliance.Blue, 2, new B_RightTrenchAuto(drive, feeder, intake, launcher)));
    autoSelector.addAuto(
        new AutoOption(Alliance.Red, 2, new R_RightTrenchAuto(drive, feeder, intake, launcher)));
    autoSelector.addAuto(
        new AutoOption(
            Alliance.Blue, 3, new B_LeftTrenchMoveFirstAuto(drive, feeder, intake, launcher)));
    autoSelector.addAuto(
        new AutoOption(
            Alliance.Red, 3, new R_LeftTrenchMoveFirstAuto(drive, feeder, intake, launcher)));
    autoSelector.addAuto(
        new AutoOption(
            Alliance.Blue, 4, new B_RightTrenchMoveFirstAuto(drive, feeder, intake, launcher)));
    autoSelector.addAuto(
        new AutoOption(
            Alliance.Red, 4, new R_RightTrenchMoveFirstAuto(drive, feeder, intake, launcher)));
  }

  public static Alliance getAlliance() {
    return allianceSelector.getAllianceColor();
  }

  private Command createDesaturateAndShootCommand(DriverController driver) {
    return Commands.parallel(
        DriveCommands.joystickDrive(
                drive,
                driver::getXTranslationInput,
                driver::getYTranslationInput,
                // launcher::desaturateTurret,
                () -> {
                  if (launcher.isTurretDesaturated()) {
                    return driver.getRotationInput();
                  } else {
                    return launcher.getTurretDesaturationDelta();
                  }
                },
                driver::getFieldRelativeInput,
                allianceSelector::fieldRotated)
            .withName("Desaturate turret"),
        Commands.sequence(
            Commands.waitUntil(launcher::isTurretDesaturated),
            Commands.startEnd(feeder::spinForward, () -> {}, feeder).withName("Advance")));
  }
}