Merge branch 'main' into auton-redirection

src/main/java/com/stuypulse/robot/Robot.java

@@ -3,8 +3,11 @@
 import com.pathplanner.lib.pathfinding.LocalADStar;
 import com.pathplanner.lib.pathfinding.Pathfinding;
 import com.stuypulse.robot.commands.BuzzController;
+import com.stuypulse.robot.commands.intake.IntakeShoot;
 import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake;
+import com.stuypulse.robot.commands.shooter.ShooterManualIntake;
 import com.stuypulse.robot.commands.vision.VisionReloadWhiteList;
+import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Settings.RobotType;
 import com.stuypulse.robot.subsystems.arm.Arm;
 import com.stuypulse.robot.subsystems.intake.Intake;
@@ -63,6 +66,23 @@ public void robotPeriodic() {
             CommandScheduler.getInstance().schedule(new ShooterAcquireFromIntake()
                                                     .andThen(new BuzzController(robot.driver)));
         }
+
+        if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
+            && Arm.getInstance().atIntakeShouldShootAngle()
+        ) {
+            CommandScheduler.getInstance().schedule(new IntakeShoot()
+                                                    .until(
+                                                        () -> Arm.getInstance().getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
+                                                            || !Arm.getInstance().atIntakeShouldShootAngle()
+                                                    ));
+        }
+
+        if (Arm.getInstance().getState() == Arm.State.AMP 
+            && !Shooter.getInstance().hasNote() 
+            && Shooter.getInstance().getFeederState() != Shooter.FeederState.DEACQUIRING
+        ) {
+            CommandScheduler.getInstance().schedule(new ShooterManualIntake().until(() -> Arm.getInstance().getState() != Arm.State.AMP));
+        }
 
         CommandScheduler.getInstance().run();
     }

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -15,7 +15,10 @@
 import com.stuypulse.robot.commands.auton.Mobility;
 import com.stuypulse.robot.commands.auton.BAC.FourPieceBAC;
 import com.stuypulse.robot.commands.intake.IntakeAcquire;
+import com.stuypulse.robot.commands.intake.IntakeAcquireForever;
 import com.stuypulse.robot.commands.intake.IntakeDeacquire;
+import com.stuypulse.robot.commands.intake.IntakeFeed;
+import com.stuypulse.robot.commands.intake.IntakeShoot;
 import com.stuypulse.robot.commands.intake.IntakeStop;
 import com.stuypulse.robot.commands.leds.LEDDefaultMode;
 import com.stuypulse.robot.commands.leds.LEDReset;
@@ -24,6 +27,7 @@
 import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire;
 import com.stuypulse.robot.commands.shooter.ShooterFeederShoot;
 import com.stuypulse.robot.commands.shooter.ShooterFeederStop;
+import com.stuypulse.robot.commands.shooter.ShooterManualIntake;
 import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker;
 import com.stuypulse.robot.commands.shooter.ShooterSetRPM;
 import com.stuypulse.robot.commands.shooter.ShooterStop;
@@ -109,6 +113,7 @@ public RobotContainer() {
 
     private void configureDefaultCommands() {
         swerve.setDefaultCommand(new SwerveDriveDrive(driver));
+        intake.setDefaultCommand(new IntakeStop());
         leds.setDefaultCommand(new LEDDefaultMode());
     }
 
@@ -127,151 +132,142 @@ private void configureDriverBindings() {
         // intake field relative
         driver.getRightTriggerButton()
             .onTrue(new ArmToFeed())
-            .whileTrue(new SwerveDriveDriveToNote(driver))
+            // .whileTrue(new SwerveDriveDriveToNote(driver))
             .whileTrue(new IntakeAcquire()
-                .deadlineWith(new LEDSet(LEDInstructions.INTAKING))
+                .deadlineWith(new LEDSet(LEDInstructions.FIELD_RELATIVE_INTAKING))
                 .andThen(new BuzzController(driver))
             );
 
         // intake robot relative
         driver.getLeftTriggerButton()
             .onTrue(new ArmToFeed())
             .whileTrue(new IntakeAcquire()
-                .deadlineWith(new LEDSet(LEDInstructions.INTAKING))
+                .deadlineWith(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING))
                 .andThen(new BuzzController(driver))
             )
             .whileTrue(new SwerveDriveDriveRobotRelative(driver));
 
         // deacquire
         driver.getDPadLeft()
-            .onTrue(new IntakeDeacquire())
-            .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING))
-            .onFalse(new IntakeStop());
+            .whileTrue(new IntakeDeacquire())
+            .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING));
 
         // speaker align and score 
         // score amp
         driver.getRightBumper()
             .whileTrue(new ConditionalCommand(
                 new ArmWaitUntilAtTarget()
                     .withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
-                    .andThen(new ShooterFeederDeacquire()),
+                    .andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE))),
                 new SwerveDriveDriveAlignedSpeaker(driver)
                     .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER))
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker()))
-                        .andThen(new ShooterFeederShoot())
+                        .andThen(new ShooterFeederShoot()
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
+                            )
                     )
-                    .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
-                                .until(() -> swerve.isAlignedToSpeaker())
-                                .andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
-                                ),
+                    .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)),
                 () -> Arm.getInstance().getState() == Arm.State.AMP))
             .onFalse(new ConditionalCommand(
                 new ShooterFeederStop(), 
                 new ShooterStop(), 
                 () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
-        // ferry align and shoot
-        // move to back of controller
-        driver.getDPadRight()
+        // lob ferry align and shoot
+        driver.getLeftStickButton()
             .whileTrue(new SwerveDriveDriveAlignedLobFerry(driver)
                     .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry()))
-                        .andThen(new ShooterFeederShoot())
+                        .andThen(new ShooterFeederShoot()
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
+                            )
                     )
-                    .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)
-                                .until(() -> swerve.isAlignedToLobFerry())
-                                .andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
-                                )
+                    .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN))
             )
             .onFalse(new ConditionalCommand(
                 new ShooterFeederStop(), 
                 new ShooterStop(), 
                 () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
-        driver.getDPadDown()
+
+        // low ferry align and shoot
+        driver.getRightStickButton()
             .whileTrue(new SwerveDriveDriveAlignedLowFerry(driver)
                     .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry()))
-                        .andThen(new ShooterFeederShoot())
+                        .andThen(new ShooterFeederShoot()
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
+                            )
                     )
-                    .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)
-                                .until(() -> swerve.isAlignedToLowFerry())
-                                .andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
-                                )
+                    .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN))
             )
             .onFalse(new ConditionalCommand(
                 new ShooterFeederStop(), 
                 new ShooterStop(), 
                 () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
-        // arm to amp and alignment
-        driver.getLeftBumper()
-            .onTrue(new ArmToAmp())
-            .onTrue(new SwerveDriveDriveAlignedAmp(driver)
-                    .onlyWhile(() -> 
-                        Math.abs(driver.getRightX()) < Settings.Driver.Turn.DISABLE_ALIGNMENT_DEADBAND.getAsDouble() && 
-                        Arm.getInstance().getState() == Arm.State.AMP)
-                    .deadlineWith(new LEDSet(LEDInstructions.AMP_WITH_ALIGN)));
+        // arm to amp
+        driver.getLeftBumper().onTrue(new ArmToAmp());
 
         // manual speaker at subwoofer
-        // rebind to a button on the back later
         driver.getRightMenuButton()
             .whileTrue(new ArmToSubwooferShot().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER))
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
-                        .andThen(new ShooterFeederShoot()))
+                        .andThen(new ShooterFeederShoot()
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
+                            )
+                        )
             .whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL))
             .onFalse(new ConditionalCommand(
                 new ShooterFeederStop(), 
                 new ShooterStop(), 
                 () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
-        // manual ferry
+        // manual lob ferry
         driver.getTopButton()
             .whileTrue(new SwerveDriveDriveAlignedManualLobFerry(driver)
                     .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLobFerry()))
-                        .andThen(new ShooterFeederShoot())
+                        .andThen(new ShooterFeederShoot()
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
+                            )
                     )
-                    .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)
-                                .until(() -> swerve.isAlignedToManualLobFerry())
-                                .andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
-                                )
+                    .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL))
             )
             .onFalse(new ConditionalCommand(
                 new ShooterFeederStop(), 
                 new ShooterStop(), 
                 () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
+        // manual low ferry
         driver.getLeftButton()
             .whileTrue(new SwerveDriveDriveAlignedManualLowFerry(driver)
                     .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLowFerry()))
-                        .andThen(new ShooterFeederShoot())
+                        .andThen(new ShooterFeederShoot()
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
+                        )
                     )
-                    .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)
-                                .until(() -> swerve.isAlignedToManualLowFerry())
-                                .andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
-                                )
+                    .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL))
             )
             .onFalse(new ConditionalCommand(
                 new ShooterFeederStop(), 
                 new ShooterStop(), 
                 () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
-        // climbing
-        driver.getRightButton().onTrue(new ArmToPreClimb());
-        driver.getBottomButton().onTrue(new ArmToClimbing());
+        // human player attention button
+        driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION));
     }
 
     private void configureOperatorBindings() {

src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java

@@ -2,9 +2,9 @@
 
 import com.stuypulse.robot.subsystems.intake.Intake;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.Command;
 
-public class IntakeDeacquire extends InstantCommand {
+public class IntakeDeacquire extends Command {
 
     private final Intake intake;
 

src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java

@@ -0,0 +1,19 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+public class IntakeShoot extends SequentialCommandGroup {
+
+    public IntakeShoot() {
+        addCommands(
+            new InstantCommand(() -> Intake.getInstance().setState(Intake.State.SHOOTING), Intake.getInstance()),
+            new WaitCommand(Settings.Intake.INTAKE_SHOOT_TIME),
+            new IntakeStop()
+        );
+    }
+}

src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java

@@ -39,15 +39,8 @@ public LEDDefaultMode() {
 
     private LEDInstruction getInstruction() {
 
-        switch (arm.getState()) {
-            case PRE_CLIMB:
-                return LEDInstructions.ARM_PRECLIMB;
-            case CLIMBING:
-                return LEDInstructions.ARM_POSTCLIMB;
-            case AMP:
-                return LEDInstructions.AMP_WITHOUT_ALIGN;
-            default:
-                break;
+        if (Arm.getInstance().getState() == Arm.State.AMP) {
+            return LEDInstructions.ARM_AT_AMP;
         }
 
         if (intake.hasNote() || shooter.hasNote()) {

src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java

@@ -29,7 +29,7 @@ public ShooterAcquireFromIntake() {
     public void initialize() {
         stopWatch.reset();
         intake.setState(Intake.State.FEEDING);
-        shooter.feederIntake();
+        shooter.setFeederState(Shooter.FeederState.INTAKING);
     }
 
     @Override
@@ -57,7 +57,7 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        shooter.feederStop();
+        shooter.setFeederState(Shooter.FeederState.STOP);
         intake.setState(Intake.State.STOP);
     }
 

src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederAcquire.java

@@ -15,7 +15,7 @@ public ShooterFeederAcquire() {
 
     @Override
     public void initialize() {
-        shooter.feederIntake();
+        shooter.setFeederState(Shooter.FeederState.INTAKING);
     }
 
 }

src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java

@@ -15,6 +15,6 @@ public ShooterFeederDeacquire() {
 
     @Override
     public void initialize() {
-        shooter.feederDeacquire();
+        shooter.setFeederState(Shooter.FeederState.DEACQUIRING);
     }
 }

src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java

@@ -15,6 +15,6 @@ public ShooterFeederShoot() {
 
     @Override
     public void initialize() {
-        shooter.feederShoot();
+        shooter.setFeederState(Shooter.FeederState.SHOOTING);
     }
 }

src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java

@@ -15,7 +15,7 @@ public ShooterFeederStop() {
 
     @Override
     public void initialize() {
-        shooter.feederStop();
+        shooter.setFeederState(Shooter.FeederState.STOP);
     }
 
 }

src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class ShooterManualIntake extends Command{
+
+    private final Shooter shooter;
+
+    public ShooterManualIntake() {
+        shooter = Shooter.getInstance();
+        addRequirements(shooter);
+    }
+
+    @Override
+    public void initialize() {
+        shooter.setFeederState(Shooter.FeederState.INTAKING);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return shooter.hasNote();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        shooter.setFeederState(Shooter.FeederState.STOP);
+    }
+}