Skip to content

Commit

Permalink
wip: breambreak changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@rutmanz
rutmanz committed Sep 14, 2024
1 parent 78ccab3 commit b5b50e6
Show file tree
Hide file tree
Showing 8 changed files with 50 additions and 110 deletions.
8 changes: 4 additions & 4 deletions src/main/java/org/team1540/robot2024/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@
public class Robot extends LoggedRobot {
private Command autonomousCommand;
private RobotContainer robotContainer;
private DigitalInput intakeBeamBreak = new DigitalInput(Constants.DIO.INTAKE_BEAM_BREAK);
private DigitalInput shooterBeamBreak = new DigitalInput(Constants.DIO.SHOOTER_BEAM_BREAK);
// private DigitalInput intakeBeamBreak = new DigitalInput(Constants.DIO.INTAKE_BEAM_BREAK);
// private DigitalInput shooterBeamBreak = new DigitalInput(Constants.DIO.SHOOTER_BEAM_BREAK);

/**
* This function is run when the robot is first started up and should be used for any
Expand Down Expand Up @@ -125,8 +125,8 @@ public void robotPeriodic() {
// the Command-based framework to work.
CommandScheduler.getInstance().run();
if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();
Logger.recordOutput("beambreak/shooter", shooterBeamBreak.get());
Logger.recordOutput("beambreak/intake", intakeBeamBreak.get());
// Logger.recordOutput("beambreak/shooter", shooterBeamBreak.get());
// Logger.recordOutput("beambreak/intake", intakeBeamBreak.get());
// Update mechanism visualiser in sim
if (Robot.isSimulation()) MechanismVisualiser.periodic();
// robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.alternating(Color.kBlueViolet, Color.kGreen));
Expand Down
5 changes: 4 additions & 1 deletion src/main/java/org/team1540/robot2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,13 +140,16 @@ private void configureButtonBindings() {
new LedPatternProgressBar(shooter::getSpinUpPercent, "#00a9ff", 33),
Leds.PatternLevel.DRIVER_LOCK));
Command overstageTargetDrive = new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter)
.alongWith(indexer.moveNoteTo(Indexer.NotePosition.SHOOTER))
.alongWith(leds.commandShowPattern(
new LedPatternProgressBar(shooter::getSpinUpPercent, "#f700ff", 33),
Leds.PatternLevel.DRIVER_LOCK));
Command counterShuffleDrive = new CounterShufflePrepareWithTargeting(driver.getHID(), drivetrain, shooter)
.alongWith(indexer.moveNoteTo(Indexer.NotePosition.SHOOTER))
.alongWith(leds.commandShowPattern(
new LedPatternProgressBar(shooter::getSpinUpPercent, "#00ffbc", 33),
Leds.PatternLevel.DRIVER_LOCK));

Command ampLock = new DriveWithAmpSideLock(drivetrain, driver.getHID())
.alongWith(leds.commandShowPattern(new LedPatternWave("#ffffff"), Leds.PatternLevel.DRIVER_LOCK));
Command cancelAlignment = Commands.runOnce(() -> {
Expand Down Expand Up @@ -208,7 +211,7 @@ private void configureButtonBindings() {
new Trigger(() -> elevator.getPosition() > 0.1).debounce(0.1)
.whileTrue(PrepareShooterCommand.lowerPivot(shooter));

new Trigger(indexer::isNoteStaged).debounce(0.05)
new Trigger(indexer.checkNoteReached(Indexer.NotePosition.INDEXER)).debounce(0.05)
.onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1))
.whileTrue(leds.commandShowIntakePattern(SimpleLedPattern.solid("#ff0000")));

Expand Down
7 changes: 2 additions & 5 deletions src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,17 +35,14 @@ public void initialize() {

@Override
public void execute() {
if (indexer.isNoteStaged()) {
if (indexer.getNoteState() == Indexer.NotePosition.INDEXER) {
indexer.stopIntake();
if (leds != null) {
leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY);
}
} else {
if (leds != null) {
if (indexer.getIntakeVoltage() == 0) {
timer.restart();
}
if (timer.hasElapsed(0.3) && indexer.getIntakeCurrent() > 30) {
if (indexer.getNoteState() == Indexer.NotePosition.INTAKE) {
leds.setPatternAll(detected, Leds.PatternLevel.INTAKE_PREREADY);
}
}
Expand Down
49 changes: 32 additions & 17 deletions src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,12 @@
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import org.team1540.robot2024.Constants;
import org.team1540.robot2024.util.LoggedTunableNumber;

import java.util.function.BooleanSupplier;

import static org.team1540.robot2024.Constants.Indexer.*;

Expand All @@ -18,10 +18,6 @@ public class Indexer extends SubsystemBase {
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);

private double feederSetpointRPM = 0.0;

private static boolean hasInstance = false;
Expand Down Expand Up @@ -68,7 +64,23 @@ public void setIntakePercent(double percent) {
}

public boolean isNoteStaged() {
return inputs.noteInIntake;
return inputs.noteInIndexer;
}

public BooleanSupplier checkNoteReached(NotePosition position) {
return () -> getNoteState().ordinal() >= position.ordinal();
}
public NotePosition getNoteState() {
if (inputs.noteInShooter) {
return NotePosition.SHOOTER;
}
if (inputs.noteInIndexer) {
return NotePosition.INDEXER;
}
if (inputs.noteInIntake) {
return NotePosition.INTAKE;
}
return NotePosition.NONE;
}

public void setFeederVelocity(double setpointRPM) {
Expand Down Expand Up @@ -114,16 +126,6 @@ public Command feedToAmp() {
public Command feedToShooter() {
return Commands.runOnce(() -> io.setFeederVelocity(1200), this);
}

public Command moveNoteOut() {
return new FunctionalCommand(
() -> setIntakePercent(-1),
() -> {},
(interrupted) -> stopIntake(),
() -> !isNoteStaged(),
this
);
}

public Command commandRunIntake(double percent) {
return Commands.startEnd(
Expand All @@ -133,6 +135,13 @@ public Command commandRunIntake(double percent) {
);
}

public Command moveNoteTo(NotePosition position) {
return Commands.startEnd(() -> {
this.setIntakePercent(1);
this.setFeederPercent(1);
}, this::stopAll).until(this.checkNoteReached(NotePosition.SHOOTER));
}

public void setIntakeBrakeMode(boolean isBrake) {
io.setIntakeBrakeMode(isBrake);
}
Expand All @@ -145,4 +154,10 @@ public double getIntakeCurrent() {
return inputs.intakeCurrentAmps;
}

public enum NotePosition {
NONE,
INTAKE,
INDEXER,
SHOOTER;
}
}
2 changes: 2 additions & 0 deletions src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,9 @@ class IndexerIOInputs {
public double feederCurrentAmps = 0.0;
public double feederVelocityRPM = 0.0;
public double feederTempCelsius = 0.0;
public boolean noteInIndexer = false;
public boolean noteInIntake = false;
public boolean noteInShooter = false;
}

/**
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ public void updateInputs(IndexerIOInputs inputs) {
inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
inputs.feederVoltage = feederVoltage;
inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
inputs.noteInIntake = indexerBeamBreak.get();
inputs.noteInIndexer = indexerBeamBreak.get();
}

@Override
Expand Down
81 changes: 0 additions & 81 deletions src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
View file

This file was deleted.

6 changes: 5 additions & 1 deletion src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@ public class IndexerIOTalonFX implements IndexerIO {
private final TalonFX intakeMotor = new TalonFX(INTAKE_ID);
private final TalonFX feederMotor = new TalonFX(FEEDER_ID);
private final DigitalInput indexerBeamBreak = new DigitalInput(Constants.DIO.INDEXER_BEAM_BREAK);
private final DigitalInput intakeBeamBreak = new DigitalInput(Constants.DIO.INTAKE_BEAM_BREAK);
private final DigitalInput shooterBeamBreak = new DigitalInput(Constants.DIO.SHOOTER_BEAM_BREAK);

private final VoltageOut feederVoltageCtrlReq = new VoltageOut(0).withEnableFOC(true);
private final VelocityVoltage feederVelocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true);
Expand Down Expand Up @@ -88,7 +90,9 @@ public void updateInputs(IndexerIOInputs inputs) {
inputs.feederCurrentAmps = feederCurrent.getValueAsDouble();
inputs.feederVelocityRPM = feederVelocity.getValueAsDouble() * 60;
inputs.feederTempCelsius = feederTemp.getValueAsDouble();
inputs.noteInIntake = !indexerBeamBreak.get();
inputs.noteInIndexer = !indexerBeamBreak.get();
inputs.noteInIntake = !intakeBeamBreak.get();
inputs.noteInShooter = !shooterBeamBreak.get();
}

@Override
Expand Down

0 comments on commit b5b50e6

Please sign in to comment.