Added outaking Triggers and OutakeShooter Command

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

@@ -342,6 +342,8 @@ public static class ShooterConstants {
         public static final double RADIUS_UPPER_HUB = 0.61; // in m
         public static final double SHOOTER_HEIGHT = 0.51; // in m
         public static final double UPPER_HUB_AIMING_HEIGHT = 2.725427; // in m
+        public static final double SHOOTER_SLOW_PERCENT = 0.10;
+
 
         // Tuning Section
         public static final double DELTA_AIM_HEIGHT_FACTOR = 0.0; // TODO: Set delta aim height factor from tuning

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

@@ -28,6 +28,7 @@
 import frc.robot.commands.shooter.*;
 import frc.robot.commands.transfer.TransferIndexForward;
 import frc.robot.commands.transfer.TransferManualReverse;
+import frc.robot.commands.transfer.TransferOff;
 import frc.robot.hardware.Limelight;
 import frc.robot.helper.ControllerUtil;
 import frc.robot.helper.DPadButton;
@@ -218,6 +219,14 @@ private void configureShooter() {
         // Vibrations
         if (TRANSFER) {
             new Button(() -> transferSubsystem.getCurrentBallCount() >= MAX_BALL_COUNT).whenPressed(new WaitAndVibrateCommand(driverController, 0.1, 0.1));
+
+            new Trigger(()-> transferSubsystem.shouldOuttake(1))
+                    .whileActiveContinuous(
+                            new ParallelCommandGroup(
+                                    new OutakeShooter(shooterSubsystem),
+                                    new TransferIndexForward(transferSubsystem)
+                            )
+                    );
         }
 
         // Flywheel Vibration from the SetShooterPIDVelocityFromStateCommand
@@ -240,13 +249,22 @@ private void configureIntake() {
 
         driverRightBumper.whenHeld(new IntakeOn(intakeSubsystem)); // TODO: bad
 
-        if (TRANSFER)
+        if (TRANSFER) {
             operatorBButton.whenHeld(
                     new ParallelCommandGroup(
                             new IntakeReverse(intakeSubsystem),
                             new TransferManualReverse(transferSubsystem)
                     )
             );
+            new Trigger(()-> transferSubsystem.shouldOuttake(0))
+                    .whileActiveContinuous(
+                            new ParallelCommandGroup(
+                                    new IntakeReverse(intakeSubsystem),
+                                    new TransferManualReverse(transferSubsystem)
+                            )
+                    );
+        }
+
 
 
 

src/main/java/frc/robot/commands/shooter/OutakeShooter.java

@@ -0,0 +1,30 @@
+package frc.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class OutakeShooter extends CommandBase {
+    private ShooterSubsystem flywheelSubsystem;
+
+    public OutakeShooter(ShooterSubsystem flywheelSubsystem) {
+        this.flywheelSubsystem = flywheelSubsystem;
+
+        addRequirements(flywheelSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        flywheelSubsystem.slowFlywheel();
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        flywheelSubsystem.stopFlywheel();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -100,6 +100,10 @@ public boolean isHoodLimitSwitchPressed(){
     /**
      * Disables powers to flywheel motor, motors change to neutral/coast mode
      */
+    public void slowFlywheel(){
+        setPercentSpeed(SHOOTER_SLOW_PERCENT);
+    }
+
     public void stopFlywheel() {
         masterLeftShooterMotor.neutralOutput();
     }

src/main/java/frc/robot/subsystems/TransferSubsystem.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.intake.IntakeOn;
 import frc.robot.hardware.MuxedColorSensor;
 import frc.robot.hardware.TalonConfiguration;
 import frc.robot.hardware.TalonFXFactory;
@@ -30,8 +31,7 @@
 import static frc.robot.Constants.LEDConstants.BALL_PATTERN;
 import static frc.robot.Constants.IDConstants;
 
-import static frc.robot.Constants.SubsystemEnableFlags.BALL_COLOR_SENSOR;
-import static frc.robot.Constants.SubsystemEnableFlags.IR_SENSORS;
+import static frc.robot.Constants.SubsystemEnableFlags.*;
 import static frc.robot.Constants.TransferConstants;
 import static frc.robot.Constants.TransferConstants.*;
 
@@ -55,6 +55,7 @@ public class TransferSubsystem extends SubsystemBase {
 
     // Linked list for FIFO queue
     LinkedList<BallColor> ballColorIndex = new LinkedList<>();
+    BallColor wrongBallColor;
 
     // For Tracking Ball RPM
     public static ShooterSubsystem flywheelSubsystem;
@@ -146,6 +147,10 @@ public boolean isFull(){
         return currentBallCount >= TransferConstants.MAX_BALL_COUNT;
     }
 
+    public boolean shouldOuttake(int index){
+       return wrongBallColor == ballColorIndex.get(index);
+    }
+
     public void transferIndexSetup(){
 
         // If no IR Sensors, Disable all Sensors
@@ -208,6 +213,7 @@ private void ballIndexEnd(){
                 logger.warning("No Ball Detected in Index!");
 
             else if (redColorCountVote == blueColorCountVote)
+
                 logger.warning("Blue and Red Ball Counts are the same!\nCount: " + redColorCountVote);
 
             else if (redColorCountVote > blueColorCountVote) {
@@ -262,6 +268,7 @@ private void removeShotBallFromIndex(){
             if (ballColorIndex.getLast() == BallColor.NONE)
                 logger.warning("No Ball Color At end of index!");
 
+
             ballColorIndex.removeLast();
             ballColorIndex.addFirst(BallColor.NONE);
 
@@ -292,11 +299,13 @@ private void updateBallLEDPattern(){
         BALL_PATTERN.update(firstBallColor, secondBallColor);
     }
 
+
     private void wrongBallColorDetected(BallColor ballColorDetected){
         logger.warning("Intaked Wrong Ball Color! " +
                 "(Alliance: " + alliance +
                 ", Ball Color Intaken: " + ballColorDetected +
                 ")");
+        wrongBallColor = ballColorDetected;
     }
 
     @Override