Skip to content

Commit

Permalink
separate indexer subsystem
Browse files Browse the repository at this point in the history
  • Loading branch information
@gvaldez7206
gvaldez7206 committed Sep 28, 2024
1 parent 289bc9e commit b509274
Show file tree
Hide file tree
Showing 8 changed files with 252 additions and 160 deletions.
47 changes: 28 additions & 19 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,9 +35,12 @@
import frc.robot.subsystems.elevator_wrist.ElevatorWrist;
import frc.robot.subsystems.elevator_wrist.ElevatorWristIO;
import frc.robot.subsystems.elevator_wrist.ElevatorWristReal;
import frc.robot.subsystems.indexer.Indexer;
import frc.robot.subsystems.indexer.IndexerIO;
import frc.robot.subsystems.indexer.IndexerIOFalcon;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.intake.IntakeIO;
import frc.robot.subsystems.intake.IntakeIOFalcon;
import frc.robot.subsystems.intake.IntakeIORev;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.ShooterIO;
import frc.robot.subsystems.shooter.ShooterVortex;
Expand Down Expand Up @@ -105,6 +108,7 @@ public class RobotContainer {
private Swerve s_Swerve;
private Shooter shooter;
private Intake intake;
private Indexer indexer;
private PhotonCameraWrapper[] cameras;
private ElevatorWrist elevatorWrist;
private LEDs leds = new LEDs(Constants.LEDConstants.LED_COUNT, Constants.LEDConstants.PWM_PORT);
Expand Down Expand Up @@ -142,20 +146,23 @@ public RobotContainer(RobotRunType runtimeType) {
switch (runtimeType) {
case kReal:
shooter = new Shooter(new ShooterVortex());
intake = new Intake(new IntakeIOFalcon());
intake = new Intake(new IntakeIORev());
indexer = new Indexer(new IndexerIOFalcon());
s_Swerve = new Swerve(new SwerveReal(), cameras);
elevatorWrist = new ElevatorWrist(new ElevatorWristReal(), operator);
break;
case kSimulation:
s_Swerve = new Swerve(new SwerveIO() {}, cameras);
shooter = new Shooter(new ShooterIO() {});
intake = new Intake(new IntakeIO() {});
indexer = new Indexer(new IndexerIO() {});
elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator);
break;
default:
s_Swerve = new Swerve(new SwerveIO() {}, cameras);
shooter = new Shooter(new ShooterIO() {});
intake = new Intake(new IntakeIO() {});
indexer = new Indexer(new IndexerIO() {});
elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator);
}

Expand All @@ -182,26 +189,26 @@ public RobotContainer(RobotRunType runtimeType) {
*/
private void configureTiggerBindings() {
// Flash LEDs Purple when note in indexer
this.intake.noteInIndexer.and(this.intake.noteInIntake.negate()).onTrue(
this.indexer.noteInIndexer.and(this.intake.noteInIntake.negate()).onTrue(
new FlashingLEDColor(leds, Constants.LEDConstants.INDEXER_COLOR).withTimeout(3));
// Flash LEDs Green when note in Intake
this.intake.noteInIntake.and(this.intake.noteInIndexer.negate())
this.intake.noteInIntake.and(this.indexer.noteInIndexer.negate())
.onTrue(new FlashingLEDColor(leds, Constants.LEDConstants.INTAKE_COLOR).withTimeout(3));
// Flash LEDs White when note in indexer AND intake at the same time
this.intake.noteInIntake.and(this.intake.noteInIndexer)
this.intake.noteInIntake.and(this.indexer.noteInIndexer)
.whileTrue(new FlashingLEDColor(leds, Constants.LEDConstants.ALERT_COLOR));
// Automatically move elevator and wrist to home position after note is spit in AMP mode
OperatorState.isAmpMode.and(OperatorState.isManualMode.negate())
.and(this.intake.noteInIndexer.negate().debounce(1))
.and(this.indexer.noteInIndexer.negate().debounce(1))
.onTrue(elevatorWrist.homePosition());
// Automatically move elevator and wrist to home position when intaking and don't have a
// note
this.intake.intakeActive.and(this.intake.noteInIndexer.negate())
this.intake.intakeActive.and(this.indexer.noteInIndexer.negate())
.and(this.elevatorWrist.elevatorAtHome.negate()).onTrue(elevatorWrist.homePosition());


RobotModeTriggers.autonomous().and(intake.noteInIndexer.negate())
.and(shooter.isShooting.negate()).whileTrue(CommandFactory.intakeNote(intake));
RobotModeTriggers.autonomous().and(indexer.noteInIndexer.negate())
.and(shooter.isShooting.negate()).whileTrue(CommandFactory.intakeNote(intake, indexer));
}

/**
Expand All @@ -216,13 +223,15 @@ private void configureButtonBindings() {
driver.start().onTrue(
new InstantCommand(() -> s_Swerve.resetPvInitialization()).ignoringDisable(true));
// intake forward
driver.rightTrigger().whileTrue(CommandFactory.newIntakeCommand(intake, elevatorWrist));
driver.rightTrigger()
.whileTrue(CommandFactory.newIntakeCommand(intake, indexer, elevatorWrist));
// intake backward
driver.leftTrigger().whileTrue(intake.runIntakeMotorNonStop(-1, -.20));
driver.leftTrigger()
.whileTrue(CommandFactory.runIntakeMotorNonStop(intake, indexer, -1, -.20));

/* Operator Buttons */
// spit note currently in robot through shooter
operator.x().whileTrue(CommandFactory.spit(shooter, intake));
operator.x().whileTrue(CommandFactory.spit(shooter, indexer));
// reset apriltag vision
operator.b().onTrue(new InstantCommand(() -> s_Swerve.resetPvInitialization()));
// spin up shooter
Expand All @@ -237,13 +246,13 @@ private void configureButtonBindings() {
// Constants.ElevatorWristConstants.SetPoints.HOME_ANGLE)));
// shoot note to speaker after being intaked
operator.rightTrigger().and(operator.leftTrigger().negate())
.whileTrue(CommandFactory.shootSpeaker(shooter, intake));
operator.rightTrigger().and(operator.leftTrigger()).whileTrue(intake.runIndexerMotor(1));
.whileTrue(CommandFactory.shootSpeaker(shooter, indexer));
operator.rightTrigger().and(operator.leftTrigger()).whileTrue(indexer.runIndexerMotor(1));
// set shooter to home preset position
operator.y().onTrue(elevatorWrist.homePosition());
operator.y().and(elevatorWrist.elevatorAtAmp).and(intake.noteInIndexer)
.onTrue(intake.runIntakeMotorNonStop(0, -0.2).withTimeout(2.0)
.until(intake.noteNotInIndexer.debounce(.5)));
operator.y().and(elevatorWrist.elevatorAtAmp).and(indexer.noteInIndexer)
.onTrue(CommandFactory.runIntakeMotorNonStop(intake, indexer, 0, -0.2).withTimeout(2.0)
.until(indexer.noteNotInIndexer.debounce(.5)));

// increment once through states list to next state
operator.povRight().onTrue(Commands.runOnce(() -> {
Expand All @@ -253,11 +262,11 @@ private void configureButtonBindings() {
operator.povLeft().onTrue(Commands.runOnce(() -> {
OperatorState.decrement();
}).ignoringDisable(true));
OperatorState.isAmpMode.and(intake.noteNotInIndexer.debounce(1.0))
OperatorState.isAmpMode.and(indexer.noteNotInIndexer.debounce(1.0))
.onTrue(elevatorWrist.homePosition());
// run action based on current state as incremented through operator states list
operator.a().and(OperatorState.isSpeakerMode).whileTrue(elevatorWrist.speakerPreset());
operator.a().and(OperatorState.isAmpMode).and(this.intake.noteInIndexer)
operator.a().and(OperatorState.isAmpMode).and(this.indexer.noteInIndexer)
.whileTrue(elevatorWrist.ampPosition());
operator.a().and(OperatorState.isShootWhileMoveMode).and(s_Swerve.seeAprilTag)
.whileTrue(new ShootWhileMoving(s_Swerve, driver, () -> s_Swerve.getPose(),
Expand Down
77 changes: 53 additions & 24 deletions src/main/java/frc/robot/commands/CommandFactory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants;
import frc.robot.subsystems.elevator_wrist.ElevatorWrist;
import frc.robot.subsystems.indexer.Indexer;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.swerve.Swerve;
Expand All @@ -21,13 +22,13 @@ public class CommandFactory {
* @param elevatorWrist Elevator and Wrist subsystem
* @return Returns a command
*/
public static Command runIntake(Intake intake, ElevatorWrist elevatorWrist) {
BooleanSupplier sensor = () -> intake.getIndexerBeamBrakeStatus();
public static Command runIntake(Intake intake, Indexer indexer, ElevatorWrist elevatorWrist) {
BooleanSupplier sensor = () -> indexer.getIndexerBeamBrakeStatus();
Command moveElevatorWrist =
elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
Constants.ElevatorWristConstants.SetPoints.HOME_ANGLE);
Command runIntakeIndexer =
intake.runIntakeMotor(0, Constants.IntakeConstants.INDEX_MOTOR_FORWARD);
runIntakeMotor(intake, indexer, 0, Constants.IntakeConstants.INDEX_MOTOR_FORWARD);
return moveElevatorWrist.andThen(runIntakeIndexer).unless(sensor);
}

Expand All @@ -38,8 +39,8 @@ public static Command runIntake(Intake intake, ElevatorWrist elevatorWrist) {
* @param intake Intake subsystem

Check warning on line 39 in src/main/java/frc/robot/commands/CommandFactory.java

View workflow job for this annotation

GitHub Actions / Linting 

[checkstyle] reported by reviewdog 🐶
Unused @param tag for 'intake'.

Raw Output:
/github/workspace/./src/main/java/frc/robot/commands/CommandFactory.java:39:8: warning: Unused @param tag for 'intake'. (com.puppycrawl.tools.checkstyle.checks.javadoc.JavadocMethodCheck)
* @return Returns a command
*/
public static Command shootSpeaker(Shooter shooter, Intake intake) {
Command runIndexer = intake.runIndexerMotor(1);
public static Command shootSpeaker(Shooter shooter, Indexer indexer) {
Command runIndexer = indexer.runIndexerMotor(1);
Command runshooter = shooter.shootSpeaker();
Command readytoShoot = Commands.waitUntil(() -> shooter.readyToShoot());
return runshooter.alongWith(readytoShoot.withTimeout(1).andThen(runIndexer));
Expand All @@ -52,12 +53,12 @@ public static Command shootSpeaker(Shooter shooter, Intake intake) {
* @param intake Intake subsystem
* @return Returns a command
*/
public static Command passThroughShoot(Shooter shooter, Intake intake) {
public static Command passThroughShoot(Shooter shooter, Intake intake, Indexer indexer) {
Command runshooter = shooter.shootSpeaker();
Command readytoShoot = Commands.waitUntil(() -> shooter.readyToShoot());
return runshooter
.alongWith(readytoShoot.withTimeout(1).andThen(intake.runIntakeMotorNonStop(0, 1)
.withTimeout(1).andThen(intake.runIntakeMotorNonStop(1, 1))));
return runshooter.alongWith(
readytoShoot.withTimeout(1).andThen(runIntakeMotorNonStop(intake, indexer, 0, 1)
.withTimeout(1).andThen(runIntakeMotorNonStop(intake, indexer, 1, 1))));
}

/**
Expand All @@ -67,8 +68,8 @@ public static Command passThroughShoot(Shooter shooter, Intake intake) {
* @param intake Intake Subsystem

Check warning on line 68 in src/main/java/frc/robot/commands/CommandFactory.java

View workflow job for this annotation

GitHub Actions / Linting 

[checkstyle] reported by reviewdog 🐶
Unused @param tag for 'intake'.

Raw Output:
/github/workspace/./src/main/java/frc/robot/commands/CommandFactory.java:68:8: warning: Unused @param tag for 'intake'. (com.puppycrawl.tools.checkstyle.checks.javadoc.JavadocMethodCheck)
* @return Command
*/
public static Command spit(Shooter shooter, Intake intake) {
return shooter.spit().alongWith(intake.runIndexerMotor(1.0));
public static Command spit(Shooter shooter, Indexer indexer) {
return shooter.spit().alongWith(indexer.runIndexerMotor(1.0));
}

/**
Expand All @@ -77,8 +78,8 @@ public static Command spit(Shooter shooter, Intake intake) {
* @param intake Intake Subsystem
* @return Command
*/
public static Command intakeNote(Intake intake) {
return intake.runIntakeMotor(1, .2);
public static Command intakeNote(Intake intake, Indexer indexer) {
return runIntakeMotor(intake, indexer, 1, .2);
}

/**
Expand All @@ -101,15 +102,16 @@ public static Command autoAngleWristSpeaker(ElevatorWrist elevatorWrist, Swerve
* @param elevatorWrist Elevator Wrist Subsystem
* @return Command
*/
public static Command newIntakeCommand(Intake intake, ElevatorWrist elevatorWrist) {
Command regularIntake = intakeNote(intake);
public static Command newIntakeCommand(Intake intake, Indexer indexer,
ElevatorWrist elevatorWrist) {
Command regularIntake = intakeNote(intake, indexer);
Command altIntake = Commands
.startEnd(() -> intake.setIntakeMotor(1), () -> intake.setIntakeMotor(0), intake)
.until(intake.noteInIntake)
.andThen(Commands.waitUntil(elevatorWrist.elevatorAtHome), intakeNote(intake));
.andThen(Commands.waitUntil(elevatorWrist.elevatorAtHome), intakeNote(intake, indexer));

return Commands.either(regularIntake, altIntake, elevatorWrist.elevatorAtHome)
.unless(intake.noteInIndexer);
.unless(indexer.noteInIndexer);
}

/**
Expand All @@ -124,9 +126,9 @@ public class Auto {
* @param intake Intake Subsystem

Check warning on line 126 in src/main/java/frc/robot/commands/CommandFactory.java

View workflow job for this annotation

GitHub Actions / Linting 

[checkstyle] reported by reviewdog 🐶
Unused @param tag for 'intake'.

Raw Output:
/github/workspace/./src/main/java/frc/robot/commands/CommandFactory.java:126:12: warning: Unused @param tag for 'intake'. (com.puppycrawl.tools.checkstyle.checks.javadoc.JavadocMethodCheck)
* @return Command
*/
public static Command runIndexer(Intake intake) {
return Commands.waitUntil(intake.noteInIndexer.negate())
.andThen(Commands.waitSeconds(.25)).deadlineWith(intake.runIndexerMotor(1))
public static Command runIndexer(Indexer indexer) {
return Commands.waitUntil(indexer.noteInIndexer.negate())
.andThen(Commands.waitSeconds(.25)).deadlineWith(indexer.runIndexerMotor(1))
.withTimeout(5);
}

Expand All @@ -137,9 +139,9 @@ public static Command runIndexer(Intake intake) {
* @param intake Intake Subsystem

Check warning on line 139 in src/main/java/frc/robot/commands/CommandFactory.java

View workflow job for this annotation

GitHub Actions / Linting 

[checkstyle] reported by reviewdog 🐶
Unused @param tag for 'intake'.

Raw Output:
/github/workspace/./src/main/java/frc/robot/commands/CommandFactory.java:139:12: warning: Unused @param tag for 'intake'. (com.puppycrawl.tools.checkstyle.checks.javadoc.JavadocMethodCheck)
* @return Command
*/
public static Command runIndexer(Intake intake, Shooter shooter) {
public static Command runIndexer(Indexer indexer, Shooter shooter) {
return Commands.waitUntil(() -> shooter.readyToShoot()).withTimeout(2)
.andThen(CommandFactory.Auto.runIndexer(intake));
.andThen(CommandFactory.Auto.runIndexer(indexer));
}

/**
Expand All @@ -148,9 +150,36 @@ public static Command runIndexer(Intake intake, Shooter shooter) {
* @param intake Intake Subsystem

Check warning on line 150 in src/main/java/frc/robot/commands/CommandFactory.java

View workflow job for this annotation

GitHub Actions / Linting 

[checkstyle] reported by reviewdog 🐶
Unused @param tag for 'intake'.

Raw Output:
/github/workspace/./src/main/java/frc/robot/commands/CommandFactory.java:150:12: warning: Unused @param tag for 'intake'. (com.puppycrawl.tools.checkstyle.checks.javadoc.JavadocMethodCheck)
* @return Command
*/
public static Command waitForIntake(Intake intake) {
return Commands.waitUntil(() -> intake.getIndexerBeamBrakeStatus());
public static Command waitForIntake(Indexer indexer) {
return Commands.waitUntil(() -> indexer.getIndexerBeamBrakeStatus());
}

}

/**
* Command to run the intake motor and indexer until the sensor trips
*
* @return {@link Command} to run the intake and indexer motors
*/
public static Command runIntakeMotor(Intake intake, Indexer indexer, double intakeSpeed,
double indexerSpeed) {
return runIntakeMotorNonStop(intake, indexer, intakeSpeed, indexerSpeed)
.until(indexer.noteInIndexer).unless(indexer.noteInIndexer);
}

/**
* Command to run the intake motor and indexer until the sensor trips
*
* @return {@link Command} to run the intake and indexer motors
*/
public static Command runIntakeMotorNonStop(Intake intake, Indexer indexer, double intakeSpeed,
double indexerSpeed) {
return Commands.startEnd(() -> {
intake.setIntakeMotor(intakeSpeed);
indexer.setIndexerMotor(indexerSpeed);
}, () -> {
intake.setIntakeMotor(0);
indexer.setIndexerMotor(0);
}, intake, indexer);
}
}
89 changes: 89 additions & 0 deletions src/main/java/frc/robot/subsystems/indexer/Indexer.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
package frc.robot.subsystems.indexer;

import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.RobotContainer;

/**
* Intake Subsystem
*/
public class Indexer extends SubsystemBase {
private IndexerIO io;
private IndexerInputsAutoLogged indexerAutoLogged = new IndexerInputsAutoLogged();

// private GenericEntry beamBrake = RobotContainer.mainDriverTab.add("Have Note", false)
// .withWidget(BuiltInWidgets.kBooleanBox).withPosition(9, 4).withSize(3, 2).getEntry();

private String noNote = Color.kBlack.toHexString();
private GenericEntry haveNote = RobotContainer.mainDriverTab.add("Have Note", noNote)
.withWidget("Single Color View").withPosition(9, 4).withSize(3, 2).getEntry();


public Trigger noteInIndexer = new Trigger(() -> getIndexerBeamBrakeStatus()).debounce(0.25,
Debouncer.DebounceType.kRising);
public Trigger noteNotInIndexer = new Trigger(() -> !getIndexerBeamBrakeStatus());
public Trigger indexerActive = new Trigger(() -> getIndexerRPM() > 0);

public Indexer(IndexerIO io) {
this.io = io;
io.updateInputs(indexerAutoLogged);
}

@Override
public void periodic() {
io.updateInputs(indexerAutoLogged);
Logger.processInputs("Indexer", indexerAutoLogged);
// if (getIndexerBeamBrakeStatus() && getintakeBeamBrakeStatus()) {
// haveNote.setString(Constants.LEDConstants.ALERT_COLOR.toHexString());
// } else if (getIndexerBeamBrakeStatus()) {
// haveNote.setString(Constants.LEDConstants.INDEXER_COLOR.toHexString());
// } else if (getintakeBeamBrakeStatus()) {
// haveNote.setString(Constants.LEDConstants.INTAKE_COLOR.toHexString());
// } else {
// haveNote.setString(noNote);
// }
}

/**
* Set the power for the indexer motor
*
* @param percentage 0-1 power for the indexer motor
*/
public void setIndexerMotor(double percentage) {
Logger.recordOutput("/Intake/Indexer Percentage", percentage);
io.setIndexerMotorPercentage(percentage);
}

/**
* Get the status of the indexer beam brake.
*
* @return True if beam brake is broken, False if open
*/
public boolean getIndexerBeamBrakeStatus() {
return indexerAutoLogged.indexerBeamBrake;
}


public double getIndexerRPM() {
return indexerAutoLogged.indexerRPM;
}

/**
* Command to run the indexer
*
* @return {@link Command} to run the indexer motors
*/
public Command runIndexerMotor(double speed) {
return Commands.startEnd(() -> {
setIndexerMotor(speed);
}, () -> {
setIndexerMotor(0);
}, this);
}
}
Loading

0 comments on commit b509274

Please sign in to comment.