From ca52be15403ea76f92ae3fed129eeb3e48537f52 Mon Sep 17 00:00:00 2001 From: Rong Date: Sat, 30 Mar 2024 11:52:08 -0700 Subject: [PATCH 1/4] Added a new command group and changed brd&btr auto --- ...ShotFromMidShootThenShootNearestThree.java | 37 ++++++++--------- ...SubwooferAndFireIfHasNoteCommandGroup.java | 40 +++++++++++++++++++ 2 files changed, 56 insertions(+), 21 deletions(-) create mode 100644 src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java diff --git a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java index 35bcbde7..77193bdd 100644 --- a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java +++ b/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,15 @@ public class SubwooferShotFromMidShootThenShootNearestThree extends SequentialCo final AutonomousCommandSelector autoSelector; double interstageTimeout = 3.5; + CollectorSubsystem collector; @Inject public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector autoSelector, Provider driveToGivenNoteAndCollectCommandGroupProvider, Provider fireFromSubwooferCommandGroup, Provider driveToCentralSubwooferCommandProvider, - PoseSubsystem pose, DriveSubsystem drive) { + PoseSubsystem pose, DriveSubsystem drive, CollectorSubsystem collector, + Provider driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider) { this.autoSelector = autoSelector; // Force our location @@ -52,13 +57,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 +71,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(driveBackIfNoteAndFireFirst); + - // 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 +95,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) { diff --git a/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java b/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java new file mode 100644 index 00000000..da30f9ce --- /dev/null +++ b/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java @@ -0,0 +1,40 @@ +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 { + + // Kinda ugly, have to hard code interstageTimeout because we can't + // get the value from our brd&btr command group + // because then it's gonna be importing each other + // any way to solve this? I feel like I run into this type of problem a lot + double interstageTimeout = 3.5; + CollectorSubsystem collector; + + @Inject + public DriveToCentralSubwooferAndFireIfHasNoteCommandGroup(FireFromSubwooferCommandGroup fireFromSubwooferCommandGroup, + DriveToCentralSubwooferCommand driveToCentralSubwooferCommand) { + var driveBackToSubwooferIfHasTopSpike = new ConditionalCommand( + driveToCentralSubwooferCommand.withTimeout(interstageTimeout), + new InstantCommand(), + this::getContainsNote + ); + var fireNoteIfHasNote = new ConditionalCommand( + fireFromSubwooferCommandGroup, + new InstantCommand(), + this::getContainsNote + ); + this.addCommands(driveBackToSubwooferIfHasTopSpike, fireNoteIfHasNote); + } + + private boolean getContainsNote() { + return collector.confidentlyHasControlOfNote(); + } +} From a03f534e84bccf23b03d91f39bfd094ee81bd45c Mon Sep 17 00:00:00 2001 From: Rong Date: Sat, 30 Mar 2024 12:07:46 -0700 Subject: [PATCH 2/4] Grouped drive and fire together --- ...ShotFromMidShootThenShootNearestThree.java | 5 ++-- ...ToCentralSubwooferAndFireCommandGroup.java | 18 ++++++++++++++ ...SubwooferAndFireIfHasNoteCommandGroup.java | 24 +++++++------------ 3 files changed, 29 insertions(+), 18 deletions(-) create mode 100644 src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java diff --git a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java index 77193bdd..4d96223c 100644 --- a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java +++ b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java @@ -34,7 +34,8 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector Provider fireFromSubwooferCommandGroup, Provider driveToCentralSubwooferCommandProvider, PoseSubsystem pose, DriveSubsystem drive, CollectorSubsystem collector, - Provider driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider) { + Provider + driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider) { this.autoSelector = autoSelector; // Force our location @@ -73,7 +74,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector // Go back and fire if has note var driveBackIfNoteAndFireSecond = driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider.get(); - this.addCommands(driveBackIfNoteAndFireFirst); + this.addCommands(driveBackIfNoteAndFireSecond); diff --git a/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java b/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java new file mode 100644 index 00000000..8a6c7ade --- /dev/null +++ b/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); + } +} diff --git a/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java b/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java index da30f9ce..5ffda940 100644 --- a/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java +++ b/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java @@ -11,30 +11,22 @@ public class DriveToCentralSubwooferAndFireIfHasNoteCommandGroup extends SequentialCommandGroup { - // Kinda ugly, have to hard code interstageTimeout because we can't - // get the value from our brd&btr command group - // because then it's gonna be importing each other - // any way to solve this? I feel like I run into this type of problem a lot - double interstageTimeout = 3.5; CollectorSubsystem collector; @Inject - public DriveToCentralSubwooferAndFireIfHasNoteCommandGroup(FireFromSubwooferCommandGroup fireFromSubwooferCommandGroup, - DriveToCentralSubwooferCommand driveToCentralSubwooferCommand) { - var driveBackToSubwooferIfHasTopSpike = new ConditionalCommand( - driveToCentralSubwooferCommand.withTimeout(interstageTimeout), + 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 ); - var fireNoteIfHasNote = new ConditionalCommand( - fireFromSubwooferCommandGroup, - new InstantCommand(), - this::getContainsNote - ); - this.addCommands(driveBackToSubwooferIfHasTopSpike, fireNoteIfHasNote); + this.addCommands(driveAndFireIfNote); } private boolean getContainsNote() { - return collector.confidentlyHasControlOfNote(); + return collector.getBeamBreakSensorActivated() || collector.getGamePieceInControl() + || collector.getGamePieceReady(); } } From 478717311612ec0be5c73862fa879bcb700bcd40 Mon Sep 17 00:00:00 2001 From: Rong Date: Sat, 30 Mar 2024 13:16:20 -0700 Subject: [PATCH 3/4] Removed unnecessary command group, moved the code to collector subsystem instead --- ...ShotFromMidShootThenShootNearestThree.java | 15 +++------ ...SubwooferAndFireIfHasNoteCommandGroup.java | 32 ------------------- .../collector/CollectorSubsystem.java | 22 ++++++++++++- 3 files changed, 25 insertions(+), 44 deletions(-) delete mode 100644 src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java diff --git a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java index 4d96223c..b3e18faf 100644 --- a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java +++ b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java @@ -1,17 +1,12 @@ 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; @@ -33,9 +28,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector Provider driveToGivenNoteAndCollectCommandGroupProvider, Provider fireFromSubwooferCommandGroup, Provider driveToCentralSubwooferCommandProvider, - PoseSubsystem pose, DriveSubsystem drive, CollectorSubsystem collector, - Provider - driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider) { + PoseSubsystem pose, DriveSubsystem drive, CollectorSubsystem collector) { this.autoSelector = autoSelector; // Force our location @@ -59,7 +52,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector this.addCommands(driveToMiddleSpikeNoteAndCollect.withTimeout(interstageTimeout)); // Go back and fire if has note - var driveBackIfNoteAndFireFirst = driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider.get(); + var driveBackIfNoteAndFireFirst = collector.getDriveAndFireIfNoteCommand(); this.addCommands(driveBackIfNoteAndFireFirst); // Drive to top spike note and collect @@ -73,7 +66,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector this.addCommands(driveToTopSpikeNoteAndCollect.withTimeout(interstageTimeout)); // Go back and fire if has note - var driveBackIfNoteAndFireSecond = driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider.get(); + var driveBackIfNoteAndFireSecond = collector.getDriveAndFireIfNoteCommand(); this.addCommands(driveBackIfNoteAndFireSecond); @@ -97,7 +90,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector this.addCommands(driveToBottomSpikeNoteAndCollect.withTimeout(interstageTimeout)); // Go back and fire if has note - var driveBackIfNoteAndFireThird = driveToCentralSubwooferAndFireIfHasNoteCommandGroupProvider.get(); + var driveBackIfNoteAndFireThird = collector.getDriveAndFireIfNoteCommand(); this.addCommands(driveBackIfNoteAndFireThird); } diff --git a/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java b/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java deleted file mode 100644 index 5ffda940..00000000 --- a/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireIfHasNoteCommandGroup.java +++ /dev/null @@ -1,32 +0,0 @@ -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(); - } -} diff --git a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java index 82ed68ca..aa7e95f5 100644 --- a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java +++ b/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 xbot.common.advantage.DataFrameRefreshable; import xbot.common.command.BaseSubsystem; import xbot.common.controls.actuators.XCANSparkMax; @@ -16,6 +19,7 @@ import xbot.common.properties.PropertyFactory; import javax.inject.Inject; +import javax.inject.Provider; import javax.inject.Singleton; @Singleton @@ -57,6 +61,7 @@ public enum CollectionSubstate { final DoubleProperty carefulAdvancePower; final DoubleProperty carefulAdvanceTimeout; final DoubleProperty lightToleranceTimeInterval; + Provider driveAndFireCommandProvider; double carefulAdvanceBeginTime = -Double.MAX_VALUE; FlipperSubsystem flipper; @@ -65,7 +70,8 @@ public enum CollectionSubstate { @Inject public CollectorSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory, ElectricalContract electricalContract, XDigitalInput.XDigitalInputFactory xDigitalInputFactory, - FlipperSubsystem flipper) { + FlipperSubsystem flipper, + Provider driveAndFireCommandProvider) { this.contract = electricalContract; if (contract.isCollectorReady()) { this.collectorMotor = sparkMaxFactory.createWithoutProperties(contract.getCollectorMotor(), getPrefix(), "CollectorMotor"); @@ -95,6 +101,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() { @@ -295,6 +302,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()) { From ab85ee16a09cb2e8708bfa9a2c189e7db4707f90 Mon Sep 17 00:00:00 2001 From: Rong Date: Tue, 2 Apr 2024 18:32:35 -0700 Subject: [PATCH 4/4] Match the timeout in actual auto --- .../DriveToCentralSubwooferAndFireCommandGroup.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java b/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java index 8a6c7ade..02434de1 100644 --- a/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java +++ b/src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java @@ -7,7 +7,7 @@ public class DriveToCentralSubwooferAndFireCommandGroup extends SequentialCommandGroup { - double interstageTimeout = 3.5; + double interstageTimeout = 7; @Inject DriveToCentralSubwooferAndFireCommandGroup(FireFromSubwooferCommandGroup fireFromSubwooferCommandGroup,