Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 9 commits into
base: main
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
ca52be1
Added a new command group and changed brd&btr auto
Rongrrz Mar 30, 2024
41e057b
Merge branch 'main' into Boolean-condition-for-auto
Rongrrz Mar 30, 2024
a03f534
Grouped drive and fire together
Rongrrz Mar 30, 2024
a2d4228
Merge branch 'Boolean-condition-for-auto' of https://github.com/Team4…
Rongrrz Mar 30, 2024
4787173
Removed unnecessary command group, moved the code to collector subsys…
Rongrrz Mar 30, 2024
8350e3e
Merge branch 'main' into Boolean-condition-for-auto
Rongrrz Mar 30, 2024
3f455d7
Merge branch 'main' into Boolean-condition-for-auto
Rongrrz Apr 3, 2024
ab85ee1
Match the timeout in actual auto
Rongrrz Apr 3, 2024
8d00f2d
Merge branch 'Boolean-condition-for-auto' of https://github.com/Team4…
Rongrrz Apr 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 13 additions & 24 deletions src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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
Expand All @@ -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");
Expand All @@ -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");
Expand All @@ -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) {
Expand Down
18 changes: 18 additions & 0 deletions src/main/java/competition/commandgroups/DriveToCentralSubwooferAndFireCommandGroup.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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);
}
}
22 changes: 21 additions & 1 deletion src/main/java/competition/subsystems/collector/CollectorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -19,6 +22,7 @@
import xbot.common.properties.PropertyFactory;

import javax.inject.Inject;
import javax.inject.Provider;
import javax.inject.Singleton;

@Singleton
Expand Down Expand Up @@ -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;
Expand All @@ -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",
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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()) {
Expand Down