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