RobotContainer.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import static frc.robot.subsystems.drive.DriveConstants.ppConfig;
import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM;
import static frc.robot.subsystems.vision.VisionConstants.*;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.pathfinding.Pathfinding;
import com.pathplanner.lib.util.PathPlannerLogging;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.lib.utils.LocalADStarAK;
import frc.robot.RobotState.IntakePosition;
import frc.robot.commands.auto.Autos;
import frc.robot.commands.auto.DriveToPose;
import frc.robot.commands.drive.DriveCommands;
import frc.robot.commands.drive.DriveWithDpad;
import frc.robot.subsystems.climber.Climber;
import frc.robot.subsystems.climber.ClimberIO;
import frc.robot.subsystems.drive.*;
import frc.robot.subsystems.indexer.Indexer;
import frc.robot.subsystems.indexer.IndexerIO;
import frc.robot.subsystems.indexer.IndexerIOSparkMax;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.intake.IntakeConstants;
import frc.robot.subsystems.intake.extend.IntakeExtend;
import frc.robot.subsystems.intake.extend.IntakeExtendIO;
import frc.robot.subsystems.intake.extend.IntakeExtendIOSparkMax;
import frc.robot.subsystems.intake.rollers.IntakeRollers;
import frc.robot.subsystems.intake.rollers.IntakeRollersIO;
import frc.robot.subsystems.intake.rollers.IntakeRollersIOKraken;
import frc.robot.subsystems.leds.Leds;
import frc.robot.subsystems.magicCarpet.MagicCarpet;
import frc.robot.subsystems.magicCarpet.MagicCarpetIO;
import frc.robot.subsystems.magicCarpet.MagicCarpetSparkMax;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.ShooterIO;
import frc.robot.subsystems.shooter.ShooterIOSparkMax;
import frc.robot.subsystems.vision.Vision;
import frc.robot.subsystems.vision.VisionIOPhotonVision;
import frc.robot.util.CustomTriggers;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

/**
 * This class is where the bulk of the robot should be declared. Since Command-based is a
 * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
 * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
 * subsystems, commands, and button mappings) should be declared here.
 */
public class RobotContainer {
  // Subsystems
  // private final Subsystem subsystem;
  private Drive drive;
  private Vision vision;
  private Leds leds;
  private MagicCarpet magicCarpet;
  private Indexer indexer;
  private final Orchestrator orchestrator;
  private Shooter shooter;
  private Climber climber;
  private final Intake intake;
  private IntakeRollers intakeRollers;
  private IntakeExtend intakeExtend;
  private RobotState robotState = RobotState.getInstance();
  private boolean isRobotOriented = true; // Workaround, change if needed

  // Controller
  private final CommandXboxController driverController = new CommandXboxController(0);
  private final CommandXboxController operatorController = new CommandXboxController(1);

  // Dashboard inputs
  private final LoggedDashboardChooser<Command> autoChooser;

  /** The container for the robot. Contains subsystems, OI devices, and commands. */
  public RobotContainer() {
    // Instantiate active subsystems
    if (Constants.getMode() != Constants.Mode.REPLAY) {
      switch (Constants.getRobot()) {
        case ROBOT_2025_COMP -> {
          drive =
              new Drive(
                  new GyroIOPigeon2(),
                  new ModuleIOTalonSpark(0),
                  new ModuleIOTalonSpark(1),
                  new ModuleIOTalonSpark(2),
                  new ModuleIOTalonSpark(3));
          vision =
              new Vision(
                  drive::addVisionMeasurement,
                  new VisionIOPhotonVision(camera0Name, robotToCamera0),
                  new VisionIOPhotonVision(camera1Name, robotToCamera1),
                  new VisionIOPhotonVision(camera2Name, robotToCamera2),
                  new VisionIOPhotonVision(camera3Name, robotToCamera3));
          leds = new Leds();
        }
        case ROBOT_2026_COMP -> {
          drive =
              new Drive(
                  new GyroIOPigeon2(),
                  new ModuleIOTalonSpark(0),
                  new ModuleIOTalonSpark(1),
                  new ModuleIOTalonSpark(2),
                  new ModuleIOTalonSpark(3));
          vision =
              new Vision(
                  drive::addVisionMeasurement,
                  new VisionIOPhotonVision(camera0Name, robotToCamera0),
                  new VisionIOPhotonVision(camera1Name, robotToCamera1),
                  new VisionIOPhotonVision(camera2Name, robotToCamera2),
                  new VisionIOPhotonVision(camera3Name, robotToCamera3));
          leds = new Leds();
          shooter = new Shooter(new ShooterIOSparkMax());
          magicCarpet = new MagicCarpet(new MagicCarpetSparkMax());
          indexer = new Indexer(new IndexerIOSparkMax());
          intakeRollers = new IntakeRollers(new IntakeRollersIOKraken());
          intakeExtend =
              new IntakeExtend(new IntakeExtendIOSparkMax(), operatorController.rightTrigger());
        }

        case ROBOT_SIMBOT -> {
          drive =
              new Drive(
                  new GyroIO() {},
                  new ModuleIOSim(),
                  new ModuleIOSim(),
                  new ModuleIOSim(),
                  new ModuleIOSim());

          leds = new Leds();
        }

        case ROBOT_BRIEFCASE -> {
          //          leds = new Leds();
          //    hopperBelt = new HopperBelt(new HopperBeltSparkMax());
          leds = new Leds();
        }
      }
    }

    // Instantiate missing subsystems
    if (drive == null) {
      drive =
          new Drive(
              new GyroIO() {},
              new ModuleIO() {},
              new ModuleIO() {},
              new ModuleIO() {},
              new ModuleIO() {});
    }
    if (magicCarpet == null) {
      magicCarpet = new MagicCarpet(new MagicCarpetIO() {});
    }
    if (shooter == null) {
      shooter = new Shooter(new ShooterIO() {});
    }
    if (indexer == null) {
      indexer = new Indexer(new IndexerIO() {});
    }
    if (climber == null) {
      climber = new Climber(new ClimberIO() {});
    }
    if (intakeRollers == null) {
      intakeRollers = new IntakeRollers(new IntakeRollersIO() {});
    }
    if (intakeExtend == null) {
      intakeExtend = new IntakeExtend(new IntakeExtendIO() {}, () -> false);
    }

    intake = new Intake(intakeRollers, intakeExtend);
    orchestrator =
        new Orchestrator(
            drive, magicCarpet, shooter, indexer, intake, intakeRollers, driverController);
    Autos autos = new Autos(drive, orchestrator, intake, intakeRollers, shooter);
    NamedCommands.registerCommand(
        "startIntake",
        Commands.parallel(
                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), orchestrator.preloadBalls())
            .withTimeout(10.0));
    NamedCommands.registerCommand(
        "endIntake",
        Commands.parallel(
                intakeRollers.stopIntakeCommand().withTimeout(0.05),
                indexer.stop().withTimeout(0.05))
            .withTimeout(0.05));
    NamedCommands.registerCommand(
        "spinUp",
        shooter.setTargetVelocityRadiansRepeatedly(
            Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM)));
    NamedCommands.registerCommand("feedShooter", orchestrator.feedUp());
    NamedCommands.registerCommand("bringInIntake", intake.extendToAngleAndIntake(0));
    NamedCommands.registerCommand("driveToHub", orchestrator.driveToHub());
    NamedCommands.registerCommand("driveToHubAuto", orchestrator.driveToHub().withTimeout(3.0));
    NamedCommands.registerCommand(
        "shootAuto",
        Commands.sequence(
            Commands.parallel(
                    shooter
                        .setTargetVelocityRadiansRepeatedly(
                            Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM))
                        .withTimeout(0.8),
                    intakeRollers.stopIntakeCommand(),
                    indexer.stop())
                .withTimeout(0.8),
            Commands.deadline(
                Commands.waitSeconds(3.2),
                shooter.setTargetVelocityRadiansRepeatedly(
                    Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM)),
                orchestrator.feedUp()),
            Commands.parallel(indexer.stop().withTimeout(0.05)).withTimeout(0.05)));
    AutoBuilder.configure(
        drive::getPose,
        drive::setPose,
        drive::getChassisSpeeds,
        drive::runVelocity,
        new PPHolonomicDriveController(new PIDConstants(12, 0, 0), new PIDConstants(9, 0, 0)),
        ppConfig,
        () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
        drive);
    Pathfinding.setPathfinder(new LocalADStarAK());
    PathPlannerLogging.setLogActivePathCallback(
        (activePath) -> {
          Logger.recordOutput(
              "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
        });
    PathPlannerLogging.setLogTargetPoseCallback(
        (targetPose) -> {
          Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
        });

    NamedCommands.registerCommand(
        "Drive Back Left",
        new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180))));
    NamedCommands.registerCommand(
        "Drive Over Left",
        new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180))));

    //
    // NamedCommands.registerCommand("startShooter",Commands.parallel(orchestrator.preloadBalls(),orchestrator.prepShooter()));
    //        NamedCommands.registerCommand("shoot",orchestrator.shootBalls());
    //        NamedCommands.registerCommand("shootClimb",orchestrator.shootBallsonClimb());
    //        NamedCommands.registerCommand("shootDistance",orchestrator.shootBallsAtDistance());
    //        NamedCommands.registerCommand("extend hopper and intake",intake.extendAndIntake());
    //
    //
    // NamedCommands.registerCommand("stopIntake",Commands.sequence(intake.collapseAndIntake(),Commands.waitSeconds(0.3),intake.stopIntakeCommand()));
    //        NamedCommands.registerCommand("climb",Commands.none());
    // Set up auto routines
    autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());

    autoChooser.addDefaultOption("Do Nothing", Commands.none());

    autoChooser.addOption("test path", autos.testPath());
    autoChooser.addOption("Bummmmpar", autos.Bummmmpar());
    autoChooser.addOption("Manual A-CC", autos.ACCManuelAuto());
    autoChooser.addOption("Manual A-CC-Improved", autos.ACCManuelAutoAlt());
    autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManuelAutoOverBump());
    autoChooser.addOption("Manual ADepot", autos.ADepot());
    autoChooser.addOption("Manual B-CC", autos.BCCManuelAuto());
    autoChooser.addOption("Manual B-CC-Improved", autos.BCCManuelAutoAlt());
    autoChooser.addOption("Manual B-CC-Over-Bump", autos.BCCManuelAutoOverBump());
    autoChooser.addOption("test path 2", drive.getAutonomousCommand("test path 2"));
    autoChooser.addOption("CL auto", autos.CenterA());
    autoChooser.addOption("8 Ball Auto", autos.EightBalls());
    autoChooser.addOption("Manual A-CC Complex Intake", autos.ACCManuelImprovedComplexIntake());

    autoChooser.addOption(
        "runCharacterizationQuasistatic", shooter.sysIdQuasistatic(Direction.kForward));
    autoChooser.addOption("runCharacterizationDynamic", shooter.sysIdDynamic(Direction.kForward));

    autoChooser.addOption(
        "runCharacterizationQuasistaticReverse", shooter.sysIdQuasistatic(Direction.kReverse));
    autoChooser.addOption(
        "runCharacterizationDynamicReverse", shooter.sysIdDynamic(Direction.kReverse));

    // Drive SysId
    autoChooser.addOption(
        "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive));
    autoChooser.addOption(
        "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive));
    autoChooser.addOption(
        "Drive SysId (Quasistatic Forward)",
        drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
    autoChooser.addOption(
        "Drive SysId (Quasistatic Reverse)",
        drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
    autoChooser.addOption(
        "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
    autoChooser.addOption(
        "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));

    autoChooser.addOption("pp", autos.ppACycleLeft());
    autoChooser.addOption("ppB", autos.ppBCycleRight());
    // autoChooser.addOption("ppB Cycle Right Regression", autos.ppBCycleRightRegression());
    // autoChooser.addOption("ppA Cycle Left Regression", autos.ppACycleLeftRegression());
    // Configure the button bindings
    configureButtonBindings();
  }

  /**
   * Use this method to define your button->command mappings. Buttons can be created by
   * instantiating a {@link GenericHID} or one of its subclasses ({@link
   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
   */
  private void configureButtonBindings() {
    indexer.setDefaultCommand(indexer.stop());
    shooter.setDefaultCommand(shooter.stop());
    //    shooter.setDefaultCommand(shooter.stop());

    drive.setDefaultCommand(
        DriveCommands.joystickDrive(
            drive,
            () -> -driverController.getLeftY(),
            () -> -driverController.getLeftX(),
            () -> -driverController.getRightX()));

    // Lock to 0 degrees when A button is held

    driverController
        .start()
        .onTrue(
            Commands.runOnce(
                    () ->
                        drive.setPose(
                            new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)),
                    drive)
                .ignoringDisable(true));
    new Trigger(() -> driverController.getHID().getPOV() != -1)
        .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV()));
    driverController.x().toggleOnTrue(orchestrator.aimToHub());
    driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
    driverController
        .leftBumper()
        .and(operatorController.pov(180))
        .whileTrue(intakeExtend.runIntakeExtendVolts(-4))
        .onFalse(intakeExtend.stopExtendingCommand());
    CustomTriggers.toggleIntakeUp(
            driverController.leftBumper(),
            () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED)
        .and(() -> !operatorController.pov(180).getAsBoolean())
        .toggleOnTrue(intakeExtend.extendToAngle(IntakeConstants.COLLAPSE_POS));
    CustomTriggers.toggleIntakeDown(
            driverController.leftBumper(),
            () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED)
        .and(() -> !operatorController.pov(180).getAsBoolean())
        .toggleOnTrue(intakeExtend.extendToAngle(IntakeConstants.EXTEND_POS));

    // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
    driverController.leftTrigger(0.2).toggleOnTrue(intakeRollers.intake());
    driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
    driverController
        .a()
        .and(operatorController.pov(180))
        .onTrue(intakeExtend.resetExtendPosition());
    driverController
        .rightBumper()
        .and(() -> !operatorController.pov(180).getAsBoolean())
        .toggleOnTrue(orchestrator.driveToHub());
    driverController
        .rightBumper()
        .and(operatorController.pov(180))
        .whileTrue(intakeExtend.runIntakeExtendVolts(4))
        .onFalse(intakeExtend.stopExtendingCommand());
    //    operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
    operatorController
        .rightTrigger(0.1)
        .and(() -> !operatorController.pov(0).getAsBoolean())
        .toggleOnTrue(
            orchestrator.spinUpShooterDistance(orchestrator.getShootWhileDrivingResultDistance()));
    operatorController
        .rightTrigger(0.1)
        .and(operatorController.pov(0))
        .toggleOnTrue(orchestrator.spinUpShooterHub());
    operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
    operatorController.y().whileTrue(indexer.reverse());
    operatorController.x().whileTrue(intakeRollers.outtake());

    operatorController
        .leftTrigger()
        .whileTrue(
            shooter.setTargetVelocityRadians(
                () ->
                    operatorController.getLeftY()
                        * Units.rotationsPerMinuteToRadiansPerSecond(5600)));
  }

  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
    return autoChooser.get();
  }

  public void robotPeriodic() {
    RobotState.getInstance().updateLEDState();
    orchestrator.orchestratorPeriodic();
  }
}