From 5bd8339e04829d62c114d2c2acf8e1d513b8f25a Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Mon, 5 Feb 2024 02:48:25 -0800 Subject: [PATCH] feat: made indexer probably interact better with other subsystems --- .../org/team1540/robot2024/Constants.java | 2 +- .../team1540/robot2024/RobotContainer.java | 10 +++---- .../commands/indexer/IntakeCommand.java | 27 +++++++++++-------- .../indexer/PrepareIndexerForShooter.java | 25 +++++++++++++++++ .../indexer/PrepareIndexerForTramp.java | 26 ++++++++++++++++++ .../commands/indexer/StageTrampCommand.java | 13 +++++++-- .../commands/shooter/ShootSequence.java | 4 +-- .../commands/tramp/StageTrampCommand.java | 26 ++++++++++++++++++ .../robot2024/subsystems/indexer/Indexer.java | 9 +++++-- 9 files changed, 119 insertions(+), 23 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java create mode 100644 src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForTramp.java create mode 100644 src/main/java/org/team1540/robot2024/commands/tramp/StageTrampCommand.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 97731693..16377629 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -78,7 +78,7 @@ public static class Indexer { public static final double INTAKE_MOI = 0.025; public static final double FEEDER_MOI = 0.025; public static final int BEAM_BREAK_ID = 0; - public static final double VELOCITY_ERR_TOLERANCE_RPM = 10; + public static final int VELOCITY_ERR_TOLERANCE_RPM = 10; } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 8c9d79d7..6f7840a7 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -69,7 +69,7 @@ public class RobotContainer { // TODO: testing dashboard inputs, remove for comp /** - * The container for the robot. Contains subsystems, OI devices, and commands. + * The container for the robot. Contains subsystems, IO devices, and commands. */ public RobotContainer() { switch (Constants.currentMode) { @@ -173,8 +173,8 @@ public RobotContainer() { private void configureButtonBindings() { drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); - copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.TOP)); - copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)); + indexer.setDefaultCommand(new IntakeCommand(indexer, tramp)); + driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); driver.b().onTrue( Commands.runOnce( @@ -183,10 +183,10 @@ private void configureButtonBindings() { ).ignoringDisable(true) ); + copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.TOP)); + copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)); copilot.a().onTrue(new ShootSequence(shooter, indexer)) .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter)); - - driver.a().onTrue(new IntakeCommand(indexer)); } /** diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java index 22e6f86c..6cf337fd 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java @@ -2,29 +2,34 @@ import edu.wpi.first.wpilibj2.command.Command; import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.tramp.Tramp; public class IntakeCommand extends Command { - private final Indexer indexer; + private final Tramp tramp; - public IntakeCommand(Indexer indexer) { + public IntakeCommand(Indexer indexer, Tramp tramp) { this.indexer = indexer; + this.tramp = tramp; addRequirements(indexer); } + // It should not be necessary to deal with the case of the note in the robot and not being detected, + // because the indexer will always be being used when the note is in transition (?) + // but maybe just for code safety this is good + // also it is hard to imagine that being a problem + // @zach @Override - public void initialize() { - indexer.setIntakePercent(0.4); - } - - - @Override - public boolean isFinished() { - return indexer.isNoteStaged(); + public void execute() { + if (indexer.isNoteStaged() || tramp.isNoteStaged()) { + indexer.stopIntake(); + } else { + indexer.setIntakePercent(0.5); + } } @Override public void end(boolean interrupted) { - indexer.setIntakePercent(0); + indexer.stopIntake(); } } diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java b/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java new file mode 100644 index 00000000..71a299ba --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java @@ -0,0 +1,25 @@ +package org.team1540.robot2024.commands.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import org.team1540.robot2024.subsystems.indexer.Indexer; + +public class PrepareIndexerForShooter extends Command { + + private final Indexer indexer; + + public PrepareIndexerForShooter(Indexer indexer) { + this.indexer = indexer; + addRequirements(indexer); + } + + @Override + public void initialize() { + indexer.setFeederVelocity(1200); + } + + @Override + public boolean isFinished() { + return indexer.isFeederAtSetpoint(); + } + +} diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForTramp.java b/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForTramp.java new file mode 100644 index 00000000..c3b53005 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForTramp.java @@ -0,0 +1,26 @@ +package org.team1540.robot2024.commands.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import org.team1540.robot2024.subsystems.indexer.Indexer; + +public class PrepareIndexerForTramp extends Command { + + private final Indexer indexer; + + // I'm not sure if the tramp should be done the same as the shooter here, but I think consistency wins? + public PrepareIndexerForTramp(Indexer indexer) { + this.indexer = indexer; + addRequirements(indexer); + } + + @Override + public void initialize() { + indexer.setFeederVelocity(-600); + } + + @Override + public boolean isFinished() { + return indexer.isFeederAtSetpoint(); + } + +} diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java index 0112797d..ec288dcf 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java @@ -17,9 +17,17 @@ public StageTrampCommand(Tramp tramp, Indexer indexer) { @Override public void initialize() { - tramp.setPercent(0.5); indexer.setFeederVelocity(-600); - indexer.setIntakePercent(0.5); + } + + // This could instead be written like ShootSequence and PrepareIndexerForShooter, but perhaps that + // is more intelligent than what is needed for the tramp + @Override + public void execute() { + if (indexer.isFeederAtSetpoint()) { + tramp.setPercent(0.5); + indexer.setIntakePercent(0.5); + } } @Override @@ -30,6 +38,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { indexer.stopFeeder(); + indexer.stopIntake(); tramp.stop(); } } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java index d8a969da..39762692 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java @@ -1,8 +1,8 @@ package org.team1540.robot2024.commands.shooter; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.team1540.robot2024.commands.indexer.PrepareIndexerForShooter; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -10,7 +10,7 @@ public class ShootSequence extends SequentialCommandGroup { public ShootSequence(Shooter shooter, Indexer indexer) { addCommands( Commands.parallel( - indexer.feedToShooter(), + new PrepareIndexerForShooter(indexer), new PrepareShooterCommand(shooter) ), Commands.runOnce(() -> indexer.setIntakePercent(1), indexer) diff --git a/src/main/java/org/team1540/robot2024/commands/tramp/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/tramp/StageTrampCommand.java new file mode 100644 index 00000000..ab50e3f9 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/tramp/StageTrampCommand.java @@ -0,0 +1,26 @@ +package org.team1540.robot2024.commands.tramp; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import org.team1540.robot2024.commands.indexer.PrepareIndexerForTramp; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.tramp.Tramp; + +public class StageTrampCommand extends SequentialCommandGroup { + public StageTrampCommand(Tramp tramp, Indexer indexer) { + addCommands( + new PrepareIndexerForTramp(indexer), + Commands.parallel( + Commands.runOnce(() -> indexer.setIntakePercent(0.5), indexer), + Commands.runOnce(() -> tramp.setPercent(0.5), tramp), + Commands.waitUntil(tramp::isNoteStaged) + ), + Commands.parallel( + Commands.runOnce(indexer::stopAll), + Commands.runOnce(tramp::stop, tramp) + ) + + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 0b41a0f5..273f06ab 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -1,6 +1,5 @@ package org.team1540.robot2024.subsystems.indexer; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -59,9 +58,15 @@ public boolean isFeederAtSetpoint() { } public void stopFeeder() { - io.setFeederVoltage(0); + io.setFeederVoltage(0); // shouldn't stopFeeder probably set its velocity? } public void stopIntake() { io.setIntakeVoltage(0); } + + public void stopAll() { + io.setIntakeVoltage(0); + io.setFeederVoltage(0); + } + }