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 edu.wpi.first.units.Units.FeetPerSecond;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;

import java.util.Arrays;

import org.ironmaple.simulation.drivesims.COTS;
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;

import com.ctre.phoenix6.CANBus;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
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.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.FieldConstants;
import frc.robot.Constants.Mode;
import frc.robot.Constants.RobotType;
import frc.robot.commands.AutoCommands;
import frc.robot.commands.drive.DriveCommands;
import frc.robot.commands.robot.RobotCommands;
import frc.robot.commands.robot.StartupCmd;
import frc.robot.generated.CompTunerConstants;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.GyroIO;
import frc.robot.subsystems.drive.GyroIOMaple;
import frc.robot.subsystems.drive.GyroIOPigeon2;
import frc.robot.subsystems.drive.ModuleIOMaple;
import frc.robot.subsystems.drive.ModuleIOReplay;
import frc.robot.subsystems.drive.ModuleIOTalonFX;
import frc.robot.subsystems.hopper.Hopper;
import frc.robot.subsystems.hopper.HopperIO;
import frc.robot.subsystems.hopper.HopperIOSim;
import frc.robot.subsystems.hopper.HopperIOTalonFX;
import frc.robot.subsystems.intake.IntakeIO;
import frc.robot.subsystems.intake.IntakeIOSim;
import frc.robot.subsystems.intake.IntakeIOTalonFX;
import frc.robot.subsystems.intake.IntakeSubsystem;
import frc.robot.subsystems.shooter.HoodIO;
import frc.robot.subsystems.shooter.HoodIOServo;
import frc.robot.subsystems.shooter.HoodIOSim;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.ShooterIO;
import frc.robot.subsystems.shooter.ShooterIOSim;
import frc.robot.subsystems.shooter.ShooterIOTalonFX;
import frc.robot.subsystems.vision.AprilTagVision;
import frc.robot.subsystems.vision.CameraIO;
import frc.robot.subsystems.vision.CameraIOLimelight4;
import frc.robot.subsystems.vision.CameraIOPhotonSim;
import frc.robot.subsystems.vision.VisionConstants;
import frc.robot.util.MapleSimUtil;
import frc.robot.util.Mechanism3d;

public class RobotContainer {

    // Subsystems
    private Drive drivebase_;
    private AprilTagVision vision_;
    private IntakeSubsystem intake_;
    private Shooter shooter_;
    private Hopper hopper_;

    private CANBus roborioCANBus = new CANBus();

    // Choosers
    private final LoggedDashboardChooser<Command> autoChooser_;
    private final LoggedDashboardChooser<Command> testBindings_;

    // Trigger Devices
    private final CommandXboxController gamepad_ = new CommandXboxController(0);
    private final CommandXboxController operatorGamepad_ = new CommandXboxController(2);
    
    public RobotContainer() {
        /**
         * Subsystem setup
         */
        if (Constants.getMode() != Mode.REPLAY) {
            switch (Constants.getRobot()) {
                case SIMBOT:
                    // Sim robot, instantiate physics sim IO implementations
                    // Create and configure a drivetrain simulation configuration
                    DriveTrainSimulationConfig config = DriveTrainSimulationConfig.Default()
                        .withGyro(COTS.ofPigeon2())
                        .withSwerveModule(COTS.ofMark4(
                            DCMotor.getKrakenX60(1),
                            DCMotor.getKrakenX60(1),
                            COTS.WHEELS.COLSONS.cof,
                            2
                        ))
                        .withTrackLengthTrackWidth(
                            Meters.of(Math.abs(
                                CompTunerConstants.FrontLeft.LocationX -
                                CompTunerConstants.BackLeft.LocationX
                            )),
                            Meters.of(Math.abs(
                                CompTunerConstants.FrontLeft.LocationY -
                                CompTunerConstants.FrontRight.LocationY
                            ))
                        )
                        .withBumperSize(Inches.of(30.75), Inches.of(37.25));

                    // Add sim drivebase to simulation and where modules can get it.
                    // CALL THIS BEFORE CREATING THE DRIVEBASE!
                    MapleSimUtil.createSwerve(config, new Pose2d(2.0, 2.0, Rotation2d.kZero));
                    MapleSimUtil.createIntake();

                    drivebase_ = new Drive(
                        new GyroIOMaple(),
                        ModuleIOMaple::new,
                        CompTunerConstants.FrontLeft,
                        CompTunerConstants.FrontRight,
                        CompTunerConstants.BackLeft,
                        CompTunerConstants.BackRight,
                        CompTunerConstants.kCANBus,
                        CompTunerConstants.kSpeedAt12Volts
                    );

                    vision_ = new AprilTagVision(
                        drivebase_::addVisionMeasurement,
                        new CameraIOPhotonSim("front", VisionConstants.frontTransform, MapleSimUtil::getPosition, true),
                        new CameraIOPhotonSim("backleft", VisionConstants.backLeftTransform, MapleSimUtil::getPosition, true),
                        new CameraIOPhotonSim("backright", VisionConstants.backRightTransform, MapleSimUtil::getPosition, true)
                    );

                    intake_= new IntakeSubsystem(new IntakeIOSim(roborioCANBus));
                    shooter_ = new Shooter(new ShooterIOSim(roborioCANBus), new HoodIOSim());
                    hopper_ = new Hopper(new HopperIOSim(roborioCANBus));
                    
                    break;

                case COMPETITION:
                    drivebase_ = new Drive(
                        new GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus),
                        ModuleIOTalonFX::new,
                        CompTunerConstants.FrontLeft,
                        CompTunerConstants.FrontRight,
                        CompTunerConstants.BackLeft,
                        CompTunerConstants.BackRight,
                        CompTunerConstants.kCANBus,
                        CompTunerConstants.kSpeedAt12Volts
                    );

                    vision_ = new AprilTagVision(
                        drivebase_::addVisionMeasurement,
                        new CameraIOLimelight4("limelight-front", drivebase_::getRotation),
                        new CameraIOLimelight4("limelight-bl", drivebase_::getRotation),
                        new CameraIOLimelight4("limelight-br", drivebase_::getRotation)
                    );

                    shooter_ = new Shooter(new ShooterIOTalonFX(roborioCANBus), new HoodIOServo());

                    hopper_ = new Hopper(new HopperIOTalonFX(roborioCANBus));
                    intake_ = new IntakeSubsystem(new IntakeIOTalonFX(roborioCANBus));
                   
                    break;
            }
        }

        /**
         * Empty subsystem setup (required in replay)
         */
        if (drivebase_ == null) { // This will be null in replay, or whenever a case above leaves a subsystem uninstantiated.
            switch (Constants.getRobot()) {
                default: // SimBot or Comp Bot
                    drivebase_ = new Drive(
                        new GyroIO() {},
                        ModuleIOReplay::new,
                        CompTunerConstants.FrontLeft,
                        CompTunerConstants.FrontRight,
                        CompTunerConstants.BackLeft,
                        CompTunerConstants.BackRight,
                        CompTunerConstants.kCANBus,
                        CompTunerConstants.kSpeedAt12Volts
                    );

                    break;
            }
        }

        if (vision_ == null) {
            int numCams = switch (Constants.getRobot()) {
                default -> 3;
            };

            CameraIO[] cams = new CameraIO[numCams];
            Arrays.setAll(cams, i -> new CameraIO() {});

            vision_ = new AprilTagVision(
                drivebase_::addVisionMeasurement,
                cams
            );
        }
        
        if (intake_ == null) {
            intake_ = new IntakeSubsystem(new IntakeIO() {});
        }
        
        if (shooter_ == null) {
            shooter_ = new Shooter(new ShooterIO() {}, new HoodIO() {});
        }

        if (hopper_ == null) {
            hopper_ = new Hopper(new HopperIO() {});
        }
        
        // Force Preload Static Apriltag Layout
        for (var tag : FieldConstants.layout.getTags()) {
            System.out.println("Tag Loaded: " + tag.ID);
        }

        DriveCommands.configure(
            drivebase_,
            () -> -gamepad_.getLeftY(),
            () -> -gamepad_.getLeftX(),
            () -> -gamepad_.getRightX()
        );

        RobotState.initialize(
            drivebase_::getPose,
            drivebase_::getFieldChassisSpeeds,
            shooter_::getTuning
        );

        // Initialize the visualizers.
        Mechanism3d.measured.zero();
        Mechanism3d.setpoints.zero();

        // Maple Sim
        if (Constants.getRobot() == RobotType.SIMBOT) {
            MapleSimUtil.start();
        }

        // AutoModes
        autoChooser_ = new LoggedDashboardChooser<>("Auto Choices");

        autoChooser_.addDefaultOption("Neutral Zone Collect - Left Trench", AutoCommands.a2NZCollectAuto(drivebase_, shooter_, intake_, hopper_, false));
        autoChooser_.addOption("Neutral Zone Collect - Right Trench", AutoCommands.a2NZCollectAuto(drivebase_, shooter_, intake_, hopper_, true));
        autoChooser_.addOption("Depot Auto", AutoCommands.a3DepotAuto(drivebase_, shooter_, intake_, hopper_)); 

        autoChooser_.onChange(auto -> {
            if (auto == null) return;
            System.out.println("Auto \"" + auto.getName() + "\" selected!");
            // Anything you may want to do when the auto is selected.
        });

        // Test Bindings
        testBindings_ = new LoggedDashboardChooser<>("Test Mode Choices");

        testBindings_.addDefaultOption("Swerve Wheel Radius", DriveCommands.wheelRadiusCharacterization(drivebase_));
        testBindings_.addOption("Swerve Feedforward", DriveCommands.feedforwardCharacterization(drivebase_));
        testBindings_.addOption("Shooter Setpoints", shooter_.testCommand(hopper_));
        testBindings_.addOption("Hood Calibration", shooter_.hoodCalibration());
        testBindings_.addOption("Startup Sequence Test", new StartupCmd(intake_, hopper_, shooter_)) ;

        // Sets the selected test binding to be triggered when the A button is pressed in test mode.
        RobotModeTriggers.test().and(gamepad_.a()).toggleOnTrue(Commands.deferredProxy(testBindings_::get));

        configureBindings();
        configureDriveBindings();
    }
    
    // Bind robot actions to commands here.
    private void configureBindings() {
        // Manually deploying and undeploying the intake.
        gamepad_.start().or(operatorGamepad_.start()).onTrue(Commands.either(
            intake_.deployCmd(),
            intake_.stowCmd(),
            intake_::isIntakeStowed
        ));
       
        // Cycle through shooter tunings when the back button is pressed.
        gamepad_.back().or(operatorGamepad_.back()).onTrue(shooter_.cycleTuning()) ;

        // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it.
        gamepad_.leftTrigger().or(operatorGamepad_.leftTrigger()).whileTrue(RobotCommands.intake(intake_, hopper_));

        // While the right trigger is held, we will shoot into the hub or ferry.
        gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger())
            .whileTrue(RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_, gamepad_.a().or(operatorGamepad_.a())));

        // While the X button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it.
        operatorGamepad_.x().whileTrue(intake_.hopperEjectSequence().alongWith(hopper_.reverseFeed()));
        operatorGamepad_.y().whileTrue(RobotCommands.ejectUp(shooter_, hopper_));

        // When the shooter isnt shooting, stow it.
        shooter_.setDefaultCommand(shooter_.idleCommand());

        // Bind dashboard button to refreshing the tuning.
        var refreshTuningButton = new LoggedNetworkBoolean("/Tuning/RefreshTuning", false);
        new Trigger(refreshTuningButton::get)
            .onTrue(shooter_.reloadTunings().finallyDo(() -> refreshTuningButton.set(false)));
    }

    private void configureDriveBindings() {
        // Default command, normal field-relative drive
        drivebase_.setDefaultCommand(DriveCommands.joystickDrive().withName("JoystickDrive"));

        // Slow Mode, during left bumper
        gamepad_.leftBumper().whileTrue(
            DriveCommands.joystickDrive(
                drivebase_,
                () -> -gamepad_.getLeftY() * DriveConstants.slowModeJoystickMultiplier,
                () -> -gamepad_.getLeftX() * DriveConstants.slowModeJoystickMultiplier,
                () -> -gamepad_.getRightX() * DriveConstants.slowModeJoystickMultiplier));

        // Switch to X pattern / brake while X button is pressed
        gamepad_.x().whileTrue(drivebase_.stopWithXCmd()); 

        // Robot Relative
        gamepad_.povUp().whileTrue(
                drivebase_.runVelocityCmd(FeetPerSecond.one(), MetersPerSecond.of(0), RadiansPerSecond.zero()));

        gamepad_.povDown().whileTrue(
                drivebase_.runVelocityCmd(FeetPerSecond.one().unaryMinus(), MetersPerSecond.of(0),
                        RadiansPerSecond.zero()));

        gamepad_.povLeft().whileTrue(
                drivebase_.runVelocityCmd(MetersPerSecond.zero(), FeetPerSecond.one(), RadiansPerSecond.zero()));

        gamepad_.povRight().whileTrue(
                drivebase_.runVelocityCmd(MetersPerSecond.zero(), FeetPerSecond.one().unaryMinus(),
                        RadiansPerSecond.zero()));

        // Robot relative diagonal
        gamepad_.povUpLeft().whileTrue(
                drivebase_.runVelocityCmd(FeetPerSecond.of(0.707), FeetPerSecond.of(0.707), RadiansPerSecond.zero()));

        gamepad_.povUpRight().whileTrue(
                drivebase_.runVelocityCmd(FeetPerSecond.of(0.707), FeetPerSecond.of(-0.707), RadiansPerSecond.zero()));

        gamepad_.povDownLeft().whileTrue(
                drivebase_.runVelocityCmd(FeetPerSecond.of(-0.707), FeetPerSecond.of(0.707), RadiansPerSecond.zero()));

        gamepad_.povDownRight().whileTrue(
                drivebase_.runVelocityCmd(FeetPerSecond.of(-0.707), FeetPerSecond.of(-0.707), RadiansPerSecond.zero()));

        // Reset gyro to 0° when Y & B button is pressed
        gamepad_.y().and(gamepad_.b()).onTrue(drivebase_.resetGyroCmd());
    }
    
    public Command getAutonomousCommand() {
        return autoChooser_.get();
    }
}