001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot;
007
008import com.stuypulse.robot.commands.auton.DoNothingAuton;
009import com.stuypulse.robot.commands.auton.LBDisrupt;
010import com.stuypulse.robot.commands.auton.LBFerry;
011import com.stuypulse.robot.commands.auton.RBDisrupt;
012import com.stuypulse.robot.commands.auton.RBFerry;
013import com.stuypulse.robot.commands.auton.depot.CenterDepot;
014import com.stuypulse.robot.commands.auton.shooting.FrontHubShootPreloads;
015import com.stuypulse.robot.commands.auton.shooting.LBDumpy;
016import com.stuypulse.robot.commands.auton.shooting.RBDumpy;
017import com.stuypulse.robot.commands.compound.StopShooting;
018import com.stuypulse.robot.commands.feeder.FeederSetForward;
019import com.stuypulse.robot.commands.handoff.HandoffSetForward;
020import com.stuypulse.robot.commands.intake.IntakeAgitateFastOnce;
021import com.stuypulse.robot.commands.intake.IntakeSetIdle;
022import com.stuypulse.robot.commands.intake.IntakeSetIntake;
023import com.stuypulse.robot.commands.intake.IntakeSetOuttake;
024import com.stuypulse.robot.commands.leds.LEDDefaultCommand;
025import com.stuypulse.robot.commands.shooter.ShooterAddToBonusVelocity;
026import com.stuypulse.robot.commands.shooter.ShooterFirstShotIncrease;
027import com.stuypulse.robot.commands.shooter.ShooterResetBonusVelocity;
028import com.stuypulse.robot.commands.shooter.ShooterSetFerry;
029import com.stuypulse.robot.commands.shooter.ShooterSetManual;
030import com.stuypulse.robot.commands.shooter.ShooterSetShoot;
031import com.stuypulse.robot.commands.shooter.ShooterWaitForSpinUp;
032import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
033import com.stuypulse.robot.commands.swerve.SwerveDriveResetRotation;
034import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
035import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveAlignToFerryZone;
036import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveAlignToHub;
037import com.stuypulse.robot.constants.Field;
038import com.stuypulse.robot.constants.Ports;
039import com.stuypulse.robot.subsystems.feeder.Feeder;
040import com.stuypulse.robot.subsystems.handoff.Handoff;
041import com.stuypulse.robot.subsystems.intake.Intake;
042import com.stuypulse.robot.subsystems.leds.LEDController;
043import com.stuypulse.robot.subsystems.shooter.Shooter;
044import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
045import com.stuypulse.robot.subsystems.vision.LimelightVision;
046import com.stuypulse.robot.util.PathUtil.AutonConfig;
047
048import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
049import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
050import edu.wpi.first.wpilibj2.command.Command;
051import edu.wpi.first.wpilibj2.command.WaitCommand;
052import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
053import edu.wpi.first.wpilibj2.command.button.Trigger;
054
055/**
056 * <h2>Robot Container Class</h2>
057 * 
058 * This class is where the bulk of the robot should be declared. Since Command-based is a
059 * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
060 * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
061 * subsystems, commands, and trigger mappings) should be declared here.
062 */
063public class RobotContainer {
064
065    // Gamepads
066    public final CommandXboxController driver = new CommandXboxController(Ports.Gamepad.DRIVER);
067
068    // Subsystem
069    private final Feeder feeder = Feeder.getInstance();
070
071    private final Intake intake = Intake.getInstance();
072
073    private final Shooter shooter = Shooter.getInstance();
074
075    private final CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance();
076
077    private final LimelightVision vision = LimelightVision.getInstance();
078
079    private final LEDController leds = LEDController.getInstance();
080
081    private final Handoff handoff = Handoff.getInstance();
082
083    // Autons
084    private static SendableChooser<Command> autonChooser = new SendableChooser<>();
085
086    // Robot container
087
088    /** The container for the robot. Contains subsystems, OI devices, and commands. */
089    public RobotContainer() {
090        swerve.configureAutoBuilder();
091        configureDefaultCommands();
092        configureButtonBindings();
093        configureAutons();
094        SmartDashboard.putData("Field", Field.FIELD2D);
095    }
096
097    /** ************ */
098    /** DEFAULTS ** */
099    /** ************ */
100    private void configureDefaultCommands() {
101        swerve.setDefaultCommand(new SwerveDriveDrive(driver));
102        leds.setDefaultCommand(new LEDDefaultCommand());
103    }
104
105    /***************/
106    /*** BUTTONS ***/
107    /***************/
108
109    /**
110     * This method is used to configure button bindings for controlling the robot.
111   */
112    private void configureButtonBindings() {
113        // Trigger buttons did not work for some reason so I had to do this
114        Trigger leftTrigger = new Trigger(() -> driver.getLeftTriggerAxis() > 0.5);
115        Trigger rightTrigger = new Trigger(() -> driver.getRightTriggerAxis() > 0.5);
116
117        leftTrigger.onTrue(new IntakeSetIntake());
118        driver.leftBumper().whileTrue(new IntakeSetOuttake());
119        driver.leftBumper().onFalse(new IntakeSetIntake());
120
121        rightTrigger.whileTrue(
122                new WaitCommand(1.5).raceWith(new SwerveDriveAlignToHub())
123                .andThen(new SwerveDriveXMode())
124                .andThen(new ShooterSetShoot())
125                .andThen(new ShooterWaitForSpinUp())
126                .andThen(new HandoffSetForward()
127                .alongWith(new FeederSetForward(),
128                    new IntakeAgitateFastOnce().repeatedly(),
129                    new ShooterFirstShotIncrease())));
130        rightTrigger.onFalse(
131            new StopShooting()
132        );
133
134        // Top Left Paddle
135        driver.y().onTrue(new IntakeSetIdle());
136        
137        driver.rightBumper()
138            .whileTrue(
139                new WaitCommand(1.5).raceWith(new SwerveDriveAlignToFerryZone())
140                    .andThen(new SwerveDriveXMode())
141                    .andThen(new ShooterSetFerry())
142                    .andThen(new ShooterWaitForSpinUp())
143                    .andThen(new HandoffSetForward())
144                    .alongWith(new FeederSetForward(), 
145                        new IntakeAgitateFastOnce().repeatedly(),
146                        new ShooterFirstShotIncrease()));
147        driver.rightBumper()
148            .onFalse(
149                new StopShooting()
150            );
151        // Manual shooting possibly from in front of the tower
152        driver.a()
153            .whileTrue(
154                new SwerveDriveXMode()
155                    .andThen(new ShooterSetManual())
156                    .andThen(new ShooterWaitForSpinUp())
157                    .andThen(new HandoffSetForward().repeatedly())
158                    .alongWith(new FeederSetForward().repeatedly(), 
159                        new IntakeAgitateFastOnce().repeatedly(),
160                        new ShooterFirstShotIncrease()));
161        driver.a()
162            .onFalse(
163                new StopShooting()
164            );
165
166        driver.b()  
167            .whileTrue(
168                new SwerveDriveXMode()
169                    .andThen(new ShooterSetShoot())
170                    .andThen(new ShooterWaitForSpinUp())
171                    .andThen(new HandoffSetForward().repeatedly())
172                    .alongWith(new FeederSetForward().repeatedly(), 
173                        new IntakeAgitateFastOnce().repeatedly(),
174                        new ShooterFirstShotIncrease())
175            );
176        driver.b()
177            .onFalse(
178                new StopShooting()
179            );
180
181        // Bottom Left Paddle
182        driver.x().whileTrue(new SwerveDriveXMode());
183
184        driver.povLeft().onTrue(new SwerveDriveResetRotation());
185        
186        driver.povUp()
187            .onTrue(new ShooterAddToBonusVelocity(50));
188        driver.povDown()
189            .onTrue(new ShooterAddToBonusVelocity(-50));
190        driver.povRight()
191            .onTrue(new ShooterResetBonusVelocity());
192
193    }
194
195    /**************/
196    /*** AUTONS ***/
197    /**************/
198
199    /**
200     * This method is used to configure the autonomous commands.
201     */
202     public void configureAutons() {
203        autonChooser.addOption("Do Nothing", new DoNothingAuton());
204        // SHOOT
205
206        AutonConfig centerDepot = new AutonConfig("Center Depot", CenterDepot::new,
207            "Hub to Depot",
208            "Tower shoot");
209        centerDepot.register(autonChooser);
210
211        AutonConfig frontHubShootPreloads = new AutonConfig("Front Hub Shoot Preloads", FrontHubShootPreloads::new,
212            "Front Hub Shoot Preloads"
213        );
214        frontHubShootPreloads.register(autonChooser);
215
216        AutonConfig LB_Dumpy = new AutonConfig("LB Dumpy", LBDumpy::new,
217            "LB to N Dumpy",
218            "LB Intake Dumpy",
219            "LB Backsweep Dumpy",
220            "LB Shoot Dumpy",
221            "LB Shoot to Depot");
222        LB_Dumpy.register(autonChooser);
223
224        AutonConfig RB_Dumpy = new AutonConfig("RB Dumpy", RBDumpy::new, 
225            "RB to N Dumpy",
226            "RB Intake Dumpy",
227            "RB Backsweep Dumpy",
228            "RB Shoot Dumpy"
229        );
230        RB_Dumpy.register(autonChooser);
231
232        // FERRY
233        AutonConfig LB_Ferry = new AutonConfig("LB Disrupt", LBFerry::new, 
234            "LB to N Ferry",
235            "N to LT Ferry",
236            "LT Hub Ferry",
237            "N to Depot Ferry"
238        );
239        LB_Ferry.register(autonChooser);
240
241        AutonConfig RB_Ferry = new AutonConfig("LB Disrupt", RBFerry::new, 
242            "RB to N Ferry",
243            "N to RT Ferry",
244            "RT Hub Ferry",
245            "N to Outpost Ferry"
246        );
247        RB_Ferry.register(autonChooser);
248
249        // DISRUPT
250        AutonConfig LB_Disrupt = new AutonConfig("LB Disrupt", LBDisrupt::new, 
251            "LB to CN Disrupt",
252            "LN Disrupt Circle",
253            "LN Disrupt Circle",
254            "LB Disrupt Return"
255        );
256        LB_Disrupt.register(autonChooser);
257
258        AutonConfig RB_Disrupt = new AutonConfig("RB Disrupt", RBDisrupt::new, 
259            "RB to CN Disrupt",
260            "RN Circle Disrupt",
261            "RN Circle Disrupt",
262            "RB Disrupt Return"
263        );
264        RB_Disrupt.register(autonChooser);
265
266        // autonChooser.addOption("SysID Module Translation Dynamic Forwards",
267        // swerve.sysIdDynamic(Direction.kForward));
268        // autonChooser.addOption("SysID Module Translation Dynamic Backwards",
269        // swerve.sysIdDynamic(Direction.kReverse));
270        // autonChooser.addOption("SysID Module Translation Quasi Forwards",
271        // swerve.sysIdQuasistatic(Direction.kForward));
272        // autonChooser.addOption("SysID Module Translation Quasi Backwards",
273        // swerve.sysIdQuasistatic(Direction.kReverse));
274        // autonChooser.addOption("SysID Rotation Translation Dynamic Forwards",
275        // swerve.sysidRotationDynamic(Direction.kForward));
276        // autonChooser.addOption("SysID Rotation Translation Dynamic Backwards",
277        // swerve.sysidRotationDynamic(Direction.kReverse));
278        // autonChooser.addOption("SysID Rotation Translation Quasi Forwards",
279        // swerve.sysidRotationQuasiStatic(Direction.kForward));
280        // autonChooser.addOption("SysID Rotation Translation Quasi Backwards",
281        // swerve.sysidRotationQuasiStatic(Direction.kReverse));
282        SmartDashboard.putData("Autonomous", autonChooser);
283    }
284
285    /**
286     * Use this to pass the autonomous command to the main {@link Robot} class.
287     * @return The command to run in autonomous
288     */
289    public Command getAutonomousCommand() {
290        return autonChooser.getSelected();
291    }
292}