2024_Robot_Code/src/main/java/frc/robot/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 java.util.Optional;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Drivetrain;

public class RobotContainer {
  private static final String kAutoTabName = "Autonomous";
  private static final String kTeleopTabName = "Teloperated";

  private Drivetrain drivetrain;

  private CommandXboxController primary;
  //private CommandXboxController secondary;

  private Trigger isTeleopTrigger;

  private SendableChooser<Command> autoChooser;

  private int ampTagID;

  // TODO There's more than one source tag, how do we want to handle this?
  private int sourceTagID;

  // TODO There's more than one speaker tag, how do we want to handle this?
  private int speakerTag;

  public RobotContainer() {
    // TODO Need to provide a real initial pose
    // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all
    // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
    drivetrain = new Drivetrain(new Pose2d(), null, null);

    // An example Named Command, doesn't need to remain once we start actually adding real things
    // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
    NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());

    // TODO Specify a default auto string once we have one
    autoChooser = AutoBuilder.buildAutoChooser();

    primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
    //secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);

    isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);

    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();

    if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
      ampTagID = 5;
      sourceTagID = 9;
      speakerTag = 4; 
    } else {
      ampTagID = 6;
      sourceTagID = 1;
      speakerTag = 8;
    }

    configureBindings();
    shuffleboardSetup();
  }

  // The objective should be to have the subsystems expose methods that return commands
  // that can be bound to the triggers provided by the CommandXboxController class.
  // This mindset should help keep RobotContainer a little cleaner this year. 
  // This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example),
  // but generally reduce/avoid any situation where the keyword "new" is involved if you're working 
  // with a subsystem
  private void configureBindings() {
    isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));

    drivetrain.setDefaultCommand(
      drivetrain.teleopCommand(
        primary::getLeftY, 
        primary::getLeftX, 
        primary::getRightX, 
        OIConstants.kTeleopDriveDeadband
      )
    );

    primary.povCenter().onFalse(
      drivetrain.driveCardinal(
        primary::getLeftY, 
        primary::getLeftX, 
        primary.getHID()::getPOV,
        OIConstants.kTeleopDriveDeadband).until(
          () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
        )
    );

    // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
    primary.a().onTrue(
      drivetrain.driveAprilTagLock(
        primary::getLeftY, 
        primary::getLeftX, 
        OIConstants.kTeleopDriveDeadband, 
        ampTagID
      ).until(
        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
      )
    );

    // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
    primary.b().onTrue(
      drivetrain.driveAprilTagLock(
        primary::getLeftY, 
        primary::getLeftX,  
        OIConstants.kTeleopDriveDeadband, 
        sourceTagID
      ).until(
        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
      )
    );

    // Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back
    primary.x().onTrue(
      drivetrain.driveAprilTagLock(
        primary::getLeftY, 
        primary::getLeftX, 
        OIConstants.kTeleopDriveDeadband, 
        speakerTag
      ).until(
        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
      )
    );

    // This was originally a run while held, not sure that's really necessary, change it if need be
    primary.y().onTrue(drivetrain.zeroHeadingCommand());

    // This was originally a run while held, not sure that's really necessary, change it if need be
    primary.rightBumper().onTrue(drivetrain.setXCommand());

    /*
     * This has been added because interest has been expressed in trying field relative vs
     * robot relative control. This should <i>default</i> to field relative, but give the option
     * for the person practicing driving to push start on the driver's controller to quickly switch to
     * the other style. 
     * 
     * If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
     * mechanism will need to be devised, this will work for now. 
     */
    primary.start().onTrue(drivetrain.toggleFieldRelativeControl());
  }

  private void shuffleboardSetup() {
    ShuffleboardTab autoTab = Shuffleboard.getTab(kAutoTabName);
    autoTab.add("Autonomous Selector", autoChooser)
      .withPosition(0, 0)
      .withSize(2, 1)
      .withWidget(BuiltInWidgets.kComboBoxChooser);

    //Always select the auto tab on startup
    Shuffleboard.selectTab(kAutoTabName);

    ShuffleboardTab teleopTab = Shuffleboard.getTab(kTeleopTabName);
    teleopTab.addBoolean("Field Relative Controls Enabled?", drivetrain::isFieldRelativeControl)
      .withPosition(0, 0)
      .withSize(2, 1)
      .withWidget(BuiltInWidgets.kBooleanBox);
  }

  public Command getAutonomousCommand() {
    return autoChooser.getSelected();
  }
}