From af7ce01a54ad0f67a75a2ed7efca684aae126e5a Mon Sep 17 00:00:00 2001 From: belowAverageCoder5188 <146988904+belowAverageCoder5188@users.noreply.github.com> Date: Fri, 8 Dec 2023 16:15:23 -0800 Subject: [PATCH] Feat: Added indexer. --- .vscode/settings.json | 2 +- gradlew | 0 src/main/java/frc/robot/RobotContainer.java | 11 +++++++- .../robot/commands/IndexerDefaultCommand.java | 28 +++++++++++++++++++ .../frc/robot/commands/ShooterCommands.java | 13 +++++++-- .../frc/robot/subsystems/indexer/Indexer.java | 25 +++++++++++++++++ .../robot/subsystems/indexer/IndexerIO.java | 13 +++++++++ .../subsystems/indexer/IndexerIOReal.java | 18 ++++++++++++ 8 files changed, 106 insertions(+), 4 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/java/frc/robot/commands/IndexerDefaultCommand.java create mode 100644 src/main/java/frc/robot/subsystems/indexer/Indexer.java create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerIO.java create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 6543015..f6161e7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -36,6 +36,6 @@ "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" }, "[java]": { - "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" + "editor.defaultFormatter": "redhat.java" } } diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 37591ec..e4abfd2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; +import frc.robot.commands.IndexerDefaultCommand; import frc.robot.commands.ShooterCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; @@ -31,6 +32,9 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOSparkMax; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.indexer.IndexerIO; +import frc.robot.subsystems.indexer.IndexerIOReal; import frc.robot.subsystems.shooter.*; import frc.robot.util.Limelight; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -47,6 +51,7 @@ public class RobotContainer { private final Drive drive; private final Flywheel flywheel; private final Shooter shooter; + private final Indexer indexer; private final Limelight limelight = new Limelight("limelight"); @@ -83,6 +88,7 @@ public RobotContainer() { // new ModuleIOTalonFX(2), // new ModuleIOTalonFX(3)); // flywheel = new Flywheel(new FlywheelIOTalonFX()); + indexer = new Indexer(new IndexerIOReal()); break; case SIM: @@ -96,6 +102,7 @@ public RobotContainer() { new ModuleIOSim()); flywheel = new Flywheel(new FlywheelIOSim()); shooter = null; + indexer = null; break; default: @@ -109,6 +116,7 @@ public RobotContainer() { new ModuleIO() {}); flywheel = new Flywheel(new FlywheelIO() {}); shooter = null; + indexer = null; break; } @@ -162,6 +170,7 @@ private void configureButtonBindings() { () -> -controller.getRightX())); shooter.setDefaultCommand(new ShooterCommands.ShooterIdle(shooter)); + indexer.setDefaultCommand(new IndexerDefaultCommand(indexer)); controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); controller .b() @@ -183,7 +192,7 @@ private void configureButtonBindings() { () -> -controller.getLeftY(), () -> -controller.getLeftX(), limelight)) - .whileTrue(new ShooterCommands.Shoot(shooter,limelight)); + .whileTrue(new ShooterCommands.Shoot(shooter,limelight,indexer)); } /** diff --git a/src/main/java/frc/robot/commands/IndexerDefaultCommand.java b/src/main/java/frc/robot/commands/IndexerDefaultCommand.java new file mode 100644 index 0000000..9f4f8eb --- /dev/null +++ b/src/main/java/frc/robot/commands/IndexerDefaultCommand.java @@ -0,0 +1,28 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.indexer.Indexer; + +public class IndexerDefaultCommand extends CommandBase { + private final Indexer indexer; + + + public IndexerDefaultCommand (Indexer indexer) { + this.indexer = indexer; + addRequirements(indexer); + } + + + @Override + public void initialize() {} + + @Override + public void execute() { + indexer.indexerRun(false); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ShooterCommands.java b/src/main/java/frc/robot/commands/ShooterCommands.java index 6302cfa..378fc61 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands.java +++ b/src/main/java/frc/robot/commands/ShooterCommands.java @@ -1,6 +1,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Limelight; @@ -13,6 +14,8 @@ public static class Shoot extends Command { Limelight limelight; + Indexer indexer; + enum State { INIT, SPINUP, @@ -22,11 +25,12 @@ enum State { State state = State.SPINUP; - public Shoot(Shooter shooter, Limelight limelight) { + public Shoot(Shooter shooter, Limelight limelight, Indexer indexer) { this.shooter = shooter; - addRequirements(shooter); + addRequirements(shooter, indexer); this.limelight = limelight; + this.indexer = indexer; } @Override @@ -45,6 +49,7 @@ public void execute() { && shooter.getSecondaryVelocityRPM() == secondaryRPM && limelight.aimed()) { shooter.setLoadingVoltage(10); + indexer.indexerRun(true); state = State.SHOOTING; } } @@ -57,6 +62,10 @@ public void execute() { } @Override +public void end(boolean isInterrupted) { + indexer.indexerRun(false); +} + @Override public boolean isFinished() { return state == State.DONE; } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 0000000..cef02e0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.indexer; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Indexer extends SubsystemBase { + IndexerIO io; + IndexerIOInputsAutoLogged inputs; + + public Indexer(IndexerIO io) { + this.io = io; + } + + public void indexerRun(boolean shooting) { + if (shooting == true) { + io.setPercent(1); + } else { + io.setPercent(0.2); + } + } + + @Override + public void periodic() { + io.updateInputs(inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java new file mode 100644 index 0000000..104c7fd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO { + @AutoLog + public static class IndexerIOInputs { + public double velocityRotationsPerSecond; + } + + public default void updateInputs(IndexerIOInputs inputs) {} + public default void setPercent(double input) {} +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java new file mode 100644 index 0000000..7cc940b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.indexer; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +public class IndexerIOReal implements IndexerIO { + CANSparkMax indexerMotor = new CANSparkMax(0, MotorType.kBrushless); + + @Override + public void setPercent(double percentage) { + indexerMotor.set(percentage); + } + + @Override + public void updateInputs(IndexerIOInputs inputs) { + inputs.velocityRotationsPerSecond = indexerMotor.getEncoder().getVelocity(); + } +}