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,12 @@
 package competition.auto_programs;
 
-import competition.commandgroups.CollectSequenceCommandGroup;
 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.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import xbot.common.subsystems.autonomous.AutonomousCommandSelector;
@@ -22,14 +20,15 @@
 public class SubwooferShotFromMidShootThenShootNearestThree extends SequentialCommandGroup {
 
     final AutonomousCommandSelector autoSelector;
+    CollectorSubsystem collector;
     double interstageTimeout = 7;
 
     @Inject
     public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector autoSelector,
                                                           Provider<DriveToGivenNoteAndCollectCommandGroup> driveToGivenNoteAndCollectCommandGroupProvider,
                                                           Provider<FireFromSubwooferCommandGroup> fireFromSubwooferCommandGroup,
                                                           Provider<DriveToCentralSubwooferCommand> driveToCentralSubwooferCommandProvider,
-                                                          PoseSubsystem pose, DriveSubsystem drive) {
+                                                          PoseSubsystem pose, DriveSubsystem drive, CollectorSubsystem collector) {
         this.autoSelector = autoSelector;
 
         // Force our location
@@ -52,13 +51,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 = collector.getDriveAndFireIfNoteCommand();
+        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 +65,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 = collector.getDriveAndFireIfNoteCommand();
+        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 +89,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 = collector.getDriveAndFireIfNoteCommand();
+        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 = 7;
+
+    @Inject
+    DriveToCentralSubwooferAndFireCommandGroup(FireFromSubwooferCommandGroup fireFromSubwooferCommandGroup,
+                                               DriveToCentralSubwooferCommand driveToCentralSubwooferCommand) {
+        this.addCommands(driveToCentralSubwooferCommand.withTimeout(interstageTimeout));
+        this.addCommands(fireFromSubwooferCommandGroup);
+    }
+}

src/main/java/competition/subsystems/collector/CollectorSubsystem.java

@@ -1,10 +1,13 @@
 package competition.subsystems.collector;
 
 import com.revrobotics.CANSparkBase;
+import competition.commandgroups.DriveToCentralSubwooferAndFireCommandGroup;
 import competition.electrical_contract.ElectricalContract;
 import competition.subsystems.flipper.FlipperSubsystem;
 import competition.subsystems.oracle.NoteCollectionInfoSource;
 import competition.subsystems.oracle.NoteFiringInfoSource;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import competition.subsystems.shooter.ShooterWheelTargetSpeeds;
 import xbot.common.advantage.DataFrameRefreshable;
 import xbot.common.command.BaseSetpointSubsystem;
@@ -19,6 +22,7 @@
 import xbot.common.properties.PropertyFactory;
 
 import javax.inject.Inject;
+import javax.inject.Provider;
 import javax.inject.Singleton;
 
 @Singleton
@@ -60,6 +64,7 @@ public enum CollectionSubstate {
     final DoubleProperty carefulAdvanceSpeed;
     final DoubleProperty carefulAdvanceTimeout;
     final DoubleProperty lightToleranceTimeInterval;
+    Provider<DriveToCentralSubwooferAndFireCommandGroup> driveAndFireCommandProvider;
     double carefulAdvanceBeginTime = -Double.MAX_VALUE;
 
     FlipperSubsystem flipper;
@@ -70,7 +75,8 @@ public enum CollectionSubstate {
     @Inject
     public CollectorSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory,
                               ElectricalContract electricalContract, XDigitalInput.XDigitalInputFactory xDigitalInputFactory,
-                              FlipperSubsystem flipper) {
+                              FlipperSubsystem flipper,
+                              Provider<DriveToCentralSubwooferAndFireCommandGroup> driveAndFireCommandProvider) {
         this.contract = electricalContract;
         if (contract.isCollectorReady()) {
             this.collectorMotor = sparkMaxFactory.create(contract.getCollectorMotor(), getPrefix(), "CollectorMotor",
@@ -112,6 +118,7 @@ public CollectorSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory s
         noteInControlValidator = new TimeStableValidator(() -> 0.1); // Checks for having the note over 0.1 seconds
 
         this.flipper = flipper;
+        this.driveAndFireCommandProvider = driveAndFireCommandProvider;
     }
 
     public void resetCollectionState() {
@@ -304,6 +311,19 @@ public boolean shouldCommitToFiring() {
         return intakeState == IntakeState.FIRING && !confidentlyHasFiredNote();
     }
 
+    public boolean getContainsNote() {
+        return getBeamBreakSensorActivated() || getGamePieceInControl()
+                || getGamePieceReady();
+    }
+
+    public ConditionalCommand getDriveAndFireIfNoteCommand() {
+        return new ConditionalCommand(
+                driveAndFireCommandProvider.get(),
+                new InstantCommand(),
+                this::getContainsNote
+        );
+    }
+
     @Override
     public void periodic() {
         if (contract.isCollectorReady()) {