Skip to content

Commit

Permalink
feat: made indexer probably interact better with other subsystems
Browse files Browse the repository at this point in the history
  • Loading branch information
AdleyJ committed Feb 5, 2024
1 parent 3fd56d7 commit 5bd8339
Show file tree
Hide file tree
Showing 9 changed files with 119 additions and 23 deletions.
2 changes: 1 addition & 1 deletion src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;


}
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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(
Expand All @@ -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));
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
}
Original file line number Diff line number Diff line change
@@ -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();
}

}
Original file line number Diff line number Diff line change
@@ -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();
}

}
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -30,6 +38,7 @@ public boolean isFinished() {
@Override
public void end(boolean interrupted) {
indexer.stopFeeder();
indexer.stopIntake();
tramp.stop();
}
}
Original file line number Diff line number Diff line change
@@ -1,16 +1,16 @@
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;

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)
Expand Down
Original file line number Diff line number Diff line change
@@ -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)
)

);
}
}
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -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);
}

}

0 comments on commit 5bd8339

Please sign in to comment.