diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index beb43872..8c9d79d7 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -16,6 +16,8 @@ import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; import org.team1540.robot2024.commands.indexer.IntakeCommand; +import org.team1540.robot2024.commands.shooter.ShootSequence; +import org.team1540.robot2024.commands.shooter.TuneShooterCommand; import org.team1540.robot2024.subsystems.drive.*; import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.elevator.ElevatorIO; @@ -65,8 +67,6 @@ public class RobotContainer { public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS); // TODO: testing dashboard inputs, remove for comp - public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000); - public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -183,7 +183,7 @@ private void configureButtonBindings() { ).ignoringDisable(true) ); - copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get())) + 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/TrampCommand.java b/src/main/java/org/team1540/robot2024/commands/TrampCommand.java deleted file mode 100644 index 9b8822bd..00000000 --- a/src/main/java/org/team1540/robot2024/commands/TrampCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.team1540.robot2024.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import org.team1540.robot2024.subsystems.tramp.Tramp; - -public class TrampCommand extends Command { - - private final Tramp tramp; - - public TrampCommand(Tramp tramp) { - this.tramp = tramp; - addRequirements(tramp); - } - - @Override - public void initialize() { - tramp.setPercent(0.5); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return tramp.isBeamBreakBlocked(); - } - - @Override - public void end(boolean interrupted) { - tramp.stopTramp(); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java new file mode 100644 index 00000000..471eb17b --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java @@ -0,0 +1,35 @@ +package org.team1540.robot2024.commands.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.tramp.Tramp; + +public class StageTrampCommand extends Command { + + private final Tramp tramp; + private final Indexer indexer; + + public StageTrampCommand(Tramp tramp, Indexer indexer) { + this.tramp = tramp; + this.indexer = indexer; + addRequirements(tramp, indexer); + } + + @Override + public void initialize() { + tramp.setPercent(0.5); + indexer.setFeederVelocity(-600); + indexer.setIntakePercent(0.5); + } + + @Override + public boolean isFinished() { + return tramp.isBeamBreakBlocked(); + } + + @Override + public void end(boolean interrupted) { + indexer.stopFeeder(); + tramp.stop(); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java new file mode 100644 index 00000000..8d757fb8 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java @@ -0,0 +1,25 @@ +package org.team1540.robot2024.commands.shooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import org.team1540.robot2024.subsystems.shooter.Shooter; + +class PrepareShooterCommand extends Command { + private final Shooter shooter; + + public PrepareShooterCommand(Shooter shooter) { + this.shooter = shooter; + addRequirements(shooter); + } + @Override + public void execute() { + // TODO: Make this dynamically update based on estimated pose + shooter.setFlywheelSpeeds(1000, 1000); + shooter.setPivotPosition(Rotation2d.fromDegrees(30)); + } + + @Override + public boolean isFinished() { + return shooter.areFlywheelsSpunUp() && shooter.isPivotAtSetpoint(); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java new file mode 100644 index 00000000..d8a969da --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java @@ -0,0 +1,26 @@ +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.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; + +public class ShootSequence extends SequentialCommandGroup { + public ShootSequence(Shooter shooter, Indexer indexer) { + addCommands( + Commands.parallel( + indexer.feedToShooter(), + new PrepareShooterCommand(shooter) + ), + Commands.runOnce(() -> indexer.setIntakePercent(1), indexer) + // TODO: Add a wait for having completed the shot (steady then current spike/velocity dip and then back down?) + ); + } + + + @Override + public InterruptionBehavior getInterruptionBehavior() { + return InterruptionBehavior.kCancelIncoming; + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java new file mode 100644 index 00000000..b963ab5e --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java @@ -0,0 +1,28 @@ +package org.team1540.robot2024.commands.shooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import org.team1540.robot2024.subsystems.shooter.Shooter; + +public class TuneShooterCommand extends Command { + private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000); + private final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000); + private final LoggedDashboardNumber angleSetpoint = new LoggedDashboardNumber("Shooter/Pivot/angleSetpoint", 30); + private final Shooter shooter; + + public TuneShooterCommand(Shooter shooter) { + this.shooter = shooter; + addRequirements(shooter); + } + + public void execute() { + shooter.setFlywheelSpeeds(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()); + shooter.setPivotPosition(Rotation2d.fromDegrees(angleSetpoint.get())); + } + + public void end() { + shooter.stopFlywheels(); + shooter.stopPivot(); + } +} 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 bd4b3be3..9b5e2943 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -11,42 +11,53 @@ public class Indexer extends SubsystemBase { - private final IndexerIO indexerIO; - private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged(); + private final IndexerIO io; + private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP); private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI); private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD); - public Indexer(IndexerIO indexerIO) { - this.indexerIO = indexerIO; + public Indexer(IndexerIO io) { + this.io = io; } public void periodic() { - indexerIO.updateInputs(indexerInputs); - Logger.processInputs("Indexer", indexerInputs); + io.updateInputs(inputs); + Logger.processInputs("Indexer", inputs); if (tuningMode) { if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { - indexerIO.configureFeederPID(kP.get(), kI.get(), kD.get()); + io.configureFeederPID(kP.get(), kI.get(), kD.get()); } } } public void setIntakePercent(double percent) { - indexerIO.setIntakeVoltage(percent * 12.0); + io.setIntakeVoltage(percent * 12.0); } public boolean isNotePresent() { - return indexerInputs.noteInIntake; + return inputs.noteInIntake; + } + + public void setFeederVelocity(double setpointRPM) { + io.setFeederVelocity(setpointRPM); } public Command feedToAmp() { - return Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this); + return Commands.runOnce(() -> io.setFeederVelocity(-600), this); } public Command feedToShooter() { - return Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this); + return Commands.runOnce(() -> io.setFeederVelocity(1200), this); } + // TODO: Add method to check if feeder is spun up + public void stopFeeder() { + io.setFeederVoltage(0); + } + public void stopIntake() { + io.setIntakeVoltage(0); + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index 7fc9913b..65296bfa 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -9,6 +9,8 @@ import org.team1540.robot2024.util.LoggedTunableNumber; import org.team1540.robot2024.util.math.AverageFilter; +import java.util.function.Supplier; + import static org.team1540.robot2024.Constants.Shooter.Flywheels; import static org.team1540.robot2024.Constants.Shooter.Pivot; @@ -161,9 +163,9 @@ public boolean isPivotAtSetpoint() { pivotSetpoint.getRotations(), pivotPositionFilter.getAverage(), Pivot.ERROR_TOLERANCE.getRotations()); } - public Command spinUpCommand(double leftSetpoint, double rightSetpoint) { + public Command spinUpCommand(Supplier leftSetpoint, Supplier rightSetpoint) { return new FunctionalCommand( - () -> setFlywheelSpeeds(leftSetpoint, rightSetpoint), + () -> setFlywheelSpeeds(leftSetpoint.get(), rightSetpoint.get()), () -> {}, (ignored) -> {}, this::areFlywheelsSpunUp, @@ -178,4 +180,5 @@ public Command setPivotPositionCommand(Rotation2d setpoint) { this::isPivotAtSetpoint, this); } + } diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java index aaeff190..6a2adfe6 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java @@ -25,7 +25,7 @@ public void periodic() { Logger.processInputs("Tramp", inputs); } - public void stopTramp() { + public void stop() { io.setVoltage(0); }