Skip to content
This repository has been archived by the owner on Jan 16, 2024. It is now read-only.

Commit

Permalink
Feat: Added indexer.
Browse files Browse the repository at this point in the history
  • Loading branch information
NateLydem committed Dec 9, 2023
1 parent 4a6d945 commit af7ce01
Show file tree
Hide file tree
Showing 8 changed files with 106 additions and 4 deletions.
2 changes: 1 addition & 1 deletion .vscode/settings.json
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,6 @@
"editor.defaultFormatter": "richardwillis.vscode-spotless-gradle"
},
"[java]": {
"editor.defaultFormatter": "richardwillis.vscode-spotless-gradle"
"editor.defaultFormatter": "redhat.java"
}
}
Empty file modified gradlew
100644 → 100755
Empty file.
11 changes: 10 additions & 1 deletion src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -24,13 +24,17 @@
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;
import frc.robot.subsystems.drive.GyroIOPigeon2;
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;
Expand All @@ -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");

Expand Down Expand Up @@ -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:
Expand All @@ -96,6 +102,7 @@ public RobotContainer() {
new ModuleIOSim());
flywheel = new Flywheel(new FlywheelIOSim());
shooter = null;
indexer = null;
break;

default:
Expand All @@ -109,6 +116,7 @@ public RobotContainer() {
new ModuleIO() {});
flywheel = new Flywheel(new FlywheelIO() {});
shooter = null;
indexer = null;
break;
}

Expand Down Expand Up @@ -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()
Expand All @@ -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));
}

/**
Expand Down
28 changes: 28 additions & 0 deletions src/main/java/frc/robot/commands/IndexerDefaultCommand.java
Original file line number Diff line number Diff line change
@@ -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;
}
}
13 changes: 11 additions & 2 deletions src/main/java/frc/robot/commands/ShooterCommands.java
Original file line number Diff line number Diff line change
@@ -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;

Expand All @@ -13,6 +14,8 @@ public static class Shoot extends Command {

Limelight limelight;

Indexer indexer;

enum State {
INIT,
SPINUP,
Expand All @@ -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
Expand All @@ -45,6 +49,7 @@ public void execute() {
&& shooter.getSecondaryVelocityRPM() == secondaryRPM
&& limelight.aimed()) {
shooter.setLoadingVoltage(10);
indexer.indexerRun(true);
state = State.SHOOTING;
}
}
Expand All @@ -57,6 +62,10 @@ public void execute() {
}

@Override
public void end(boolean isInterrupted) {
indexer.indexerRun(false);
}
@Override
public boolean isFinished() {
return state == State.DONE;
}
Expand Down
25 changes: 25 additions & 0 deletions src/main/java/frc/robot/subsystems/indexer/Indexer.java
Original file line number Diff line number Diff line change
@@ -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);
}
}
13 changes: 13 additions & 0 deletions src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
Original file line number Diff line number Diff line change
@@ -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) {}
}
18 changes: 18 additions & 0 deletions src/main/java/frc/robot/subsystems/indexer/IndexerIOReal.java
Original file line number Diff line number Diff line change
@@ -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();
}
}

0 comments on commit af7ce01

Please sign in to comment.