Bread and butter 4 note auto: no longer goes back to shoot if no note in robot

src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java

@@ -1,14 +1,17 @@
 package competition.auto_programs;
 
 import competition.commandgroups.CollectSequenceCommandGroup;
+import competition.commandgroups.DriveToCentralSubwooferAndFireIfHasNoteCommandGroup;
 import competition.commandgroups.DriveToGivenNoteAndCollectCommandGroup;
 import competition.commandgroups.FireFromSubwooferCommandGroup;
+import competition.subsystems.collector.CollectorSubsystem;
 import competition.subsystems.drive.DriveSubsystem;
 import competition.subsystems.drive.commands.DriveToCentralSubwooferCommand;
 import competition.subsystems.drive.commands.DriveToListOfPointsCommand;
 import competition.subsystems.pose.PoseSubsystem;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import xbot.common.subsystems.autonomous.AutonomousCommandSelector;
@@ -23,13 +26,16 @@ public class SubwooferShotFromMidShootThenShootNearestThree extends SequentialCo
 
     final AutonomousCommandSelector autoSelector;
     double interstageTimeout = 3.5;
+    CollectorSubsystem collector;
 
     @Inject
     public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector autoSelector,
                                                           Provider<DriveToGivenNoteAndCollectCommandGroup> driveToGivenNoteAndCollectCommandGroupProvider,
                                                           Provider<FireFromSubwooferCommandGroup> fireFromSubwooferCommandGroup,
                                                           Provider<DriveToCentralSubwooferCommand> driveToCentralSubwooferCommandProvider,
-                                                          PoseSubsystem pose, DriveSubsystem drive) {
+                                                          PoseSubsystem pose, DriveSubsystem drive, CollectorSubsystem collector,
+                                                          Provider<DriveToCentralSubwooferAndFireIfHasNoteCommandGroup>
+                                                                      driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider) {
         this.autoSelector = autoSelector;
 
         // Force our location
@@ -52,13 +58,9 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector
         var driveToMiddleSpikeNoteAndCollect = driveToGivenNoteAndCollectCommandGroupProvider.get();
         this.addCommands(driveToMiddleSpikeNoteAndCollect.withTimeout(interstageTimeout));
 
-        // Drive back to subwoofer
-        var driveBackToCentralSubwooferFirst = driveToCentralSubwooferCommandProvider.get();
-        this.addCommands(driveBackToCentralSubwooferFirst.withTimeout(interstageTimeout));
-
-        // Fire second note into the speaker
-        var fireSecondNoteCommand = fireFromSubwooferCommandGroup.get();
-        this.addCommands(fireSecondNoteCommand);
+        // Go back and fire if has note
+        var driveBackIfNoteAndFireFirst = driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider.get();
+        this.addCommands(driveBackIfNoteAndFireFirst);
 
         // Drive to top spike note and collect
         queueMessageToAutoSelector("Drive to top spike note, collect, drive back to sub(middle) and shoot");
@@ -70,13 +72,11 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector
         var driveToTopSpikeNoteAndCollect = driveToGivenNoteAndCollectCommandGroupProvider.get();
         this.addCommands(driveToTopSpikeNoteAndCollect.withTimeout(interstageTimeout));
 
-        // Drive back to subwoofer
-        var driveBackToCentralSubwooferSecond = driveToCentralSubwooferCommandProvider.get();
-        this.addCommands(driveBackToCentralSubwooferSecond.withTimeout(interstageTimeout));
+        // Go back and fire if has note
+        var driveBackIfNoteAndFireSecond = driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider.get();
+        this.addCommands(driveBackIfNoteAndFireSecond);
+
 
-        // Fire Note into the speaker
-        var fireThirdNoteCommand = fireFromSubwooferCommandGroup.get();
-        this.addCommands(fireThirdNoteCommand);
 
         // Drive to bottom spike note and collect
         queueMessageToAutoSelector("Drive to bottom spike note, collect, drive back to sub(middle) and shoot");
@@ -96,13 +96,9 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector
         var driveToBottomSpikeNoteAndCollect = driveToGivenNoteAndCollectCommandGroupProvider.get();
         this.addCommands(driveToBottomSpikeNoteAndCollect.withTimeout(interstageTimeout));
 
-        // Drive back to subwoofer
-        var driveBackToCentralSubwooferThird = driveToCentralSubwooferCommandProvider.get();
-        this.addCommands(driveBackToCentralSubwooferThird.withTimeout(interstageTimeout));
-
-        // Fire Note into the speaker
-        var fireFourthNoteCommand = fireFromSubwooferCommandGroup.get();
-        this.addCommands(fireFourthNoteCommand);
+        // Go back and fire if has note
+        var driveBackIfNoteAndFireThird = driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider.get();
+        this.addCommands(driveBackIfNoteAndFireThird);
     }
 
     private void queueMessageToAutoSelector(String message) {

src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java

@@ -0,0 +1,18 @@
+package competition.commandgroups;
+
+import competition.subsystems.drive.commands.DriveToCentralSubwooferCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import javax.inject.Inject;
+
+public class DriveToCentralSubwooferAndFireCommandGroup extends SequentialCommandGroup {
+
+    double interstageTimeout = 3.5;
+
+    @Inject
+    DriveToCentralSubwooferAndFireCommandGroup(FireFromSubwooferCommandGroup fireFromSubwooferCommandGroup,
+                                               DriveToCentralSubwooferCommand driveToCentralSubwooferCommand) {
+        this.addCommands(driveToCentralSubwooferCommand.withTimeout(interstageTimeout));
+        this.addCommands(fireFromSubwooferCommandGroup);
+    }
+}

src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java

@@ -0,0 +1,32 @@
+package competition.commandgroups;
+
+import competition.subsystems.collector.CollectorSubsystem;
+import competition.subsystems.drive.commands.DriveToCentralSubwooferCommand;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import javax.inject.Inject;
+import javax.inject.Provider;
+
+public class DriveToCentralSubwooferAndFireIfHasNoteCommandGroup extends SequentialCommandGroup {
+
+    CollectorSubsystem collector;
+
+    @Inject
+    public DriveToCentralSubwooferAndFireIfHasNoteCommandGroup(DriveToCentralSubwooferAndFireCommandGroup
+                                                                           driveToCentralSubwooferAndFireCommandGroup) {
+        // Since this is only one command, I think we can probably simplify it to not be a command group?
+        var driveAndFireIfNote = new ConditionalCommand(
+                driveToCentralSubwooferAndFireCommandGroup,
+                new InstantCommand(),
+                this::getContainsNote
+        );
+        this.addCommands(driveAndFireIfNote);
+    }
+
+    private boolean getContainsNote() {
+        return collector.getBeamBreakSensorActivated() || collector.getGamePieceInControl()
+                || collector.getGamePieceReady();
+    }
+}