Added advanced sequential command

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.4.2"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
     id "com.diffplug.spotless" version "6.13.0"
     id "me.champeau.jmh" version "0.6.8"
 }

src/main/java/frc/robot/Commands/AdvancedIntakeSequence.java

@@ -0,0 +1,25 @@
+// 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.Commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
+import frc.robot.Subsystems.IntakeSubsystem;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class AdvancedIntakeSequence extends SequentialCommandGroup {
+  /** Creates a new AdvancedIntakeSequence. */
+  public AdvancedIntakeSequence(IntakeSubsystem intakeSubsystem, boolean isCone, boolean isIntake) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    if(isIntake){
+      addCommands( new IntakeCommand(intakeSubsystem, isCone, isIntake), new StopIntakeMotorCommand(intakeSubsystem));
+    }else{
+      addCommands( new IntakeCommand(intakeSubsystem, isCone, isIntake).withTimeout(Constants.Intake.UNLOADING_WAIT_TIME), new StopIntakeMotorCommand(intakeSubsystem) );
+    }
+  }
+}

src/main/java/frc/robot/Commands/IntakeCommand.java

@@ -52,6 +52,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return true;
+    return intakeSubsystem.IsLoaded();
   }
 }

src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java

@@ -36,6 +36,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return true;
   }
 }

src/main/java/frc/robot/Constants.java

@@ -27,5 +27,7 @@ public static class Intake {
 
     public static final double CUBE_STATOR_LIMIT = 90;
     public static final double CONE_STATOR_LIMIT = 70;
+
+    public static final double UNLOADING_WAIT_TIME = 2f;
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Commands.AdvancedIntakeSequence;
 import frc.robot.Commands.IntakeCommand;
 import frc.robot.Commands.StopIntakeMotorCommand;
 import frc.robot.Subsystems.IntakeSubsystem;
@@ -63,12 +64,10 @@ public void containerMatchStarting() {
    */
   private void configureButtonBindings() {
     // intake and outtake
-    driverB.rightTrigger().onTrue(new IntakeCommand(intakeSubsystem, false, true));
-    driverB.leftTrigger().onTrue(new IntakeCommand(intakeSubsystem, true, true));
-    driverB.rightBumper().onTrue(new IntakeCommand(intakeSubsystem, false, false));
-    driverB.leftBumper().onTrue(new IntakeCommand(intakeSubsystem, true, false));
+    driverB.rightTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, true));
+    driverB.leftTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, true));
+    driverB.rightBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, false));
+    driverB.leftBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, false));
 
-    // stop the motors
-    driverB.x().onTrue(new StopIntakeMotorCommand(intakeSubsystem));
   }
 }

src/main/java/frc/robot/Subsystems/IntakeSubsystem.java

@@ -67,7 +67,7 @@ public void periodic() {
     // set the motor power to the var
     intakeMotor.set(TalonFXControlMode.PercentOutput, motorPower);
 
-    // if we are done loading than set the motor power to 0
-    if (IsLoaded()) SetMotorPower(0);
+    // // if we are done loading than set the motor power to 0
+    // if (IsLoaded()) SetMotorPower(0);
   }
 }