diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 4c3cde4..2715ea6 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -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 @@ -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)); diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index a6b00b2..8b09bd9 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -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(() -> { @@ -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"))); diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java index ce644fb..d474e3a 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java @@ -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); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index db69b3a..96fb7c0 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -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.*; @@ -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; @@ -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) { @@ -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( @@ -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); } @@ -145,4 +154,10 @@ public double getIntakeCurrent() { return inputs.intakeCurrentAmps; } + public enum NotePosition { + NONE, + INTAKE, + INDEXER, + SHOOTER; + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index 3d1b8ef..26046cc 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -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; } /** diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java index 1e45a23..fbf5d07 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -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 diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java deleted file mode 100644 index df790fe..0000000 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ /dev/null @@ -1,81 +0,0 @@ -package org.team1540.robot2024.subsystems.indexer; - -import com.revrobotics.*; -import edu.wpi.first.wpilibj.DigitalInput; - -import static org.team1540.robot2024.Constants.Indexer.*; - - -public class IndexerIOSparkMax implements IndexerIO { - private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); - private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless); - private final DigitalInput indexerBeamBreak = new DigitalInput(7); - - private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); - private final RelativeEncoder feederEncoder = feederMotor.getEncoder(); - - private final SparkPIDController feederPID = feederMotor.getPIDController(); - - - public IndexerIOSparkMax() { - intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - intakeMotor.enableVoltageCompensation(12.0); - intakeMotor.setSmartCurrentLimit(55); - - feederMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - feederMotor.setInverted(true); - feederMotor.enableVoltageCompensation(12.0); - feederMotor.setSmartCurrentLimit(60); - - feederPID.setP(FEEDER_KP, 0); - feederPID.setI(FEEDER_KI, 0); - feederPID.setD(FEEDER_KD, 0); - feederPID.setFF(FEEDER_KV, 0); - } - - @Override - public void updateInputs(IndexerIOInputs inputs) { - inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); - inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); - inputs.intakeVelocityRPM = intakeEncoder.getVelocity(); - inputs.intakeTempCelsius = intakeMotor.getMotorTemperature(); - inputs.feederCurrentAmps = feederMotor.getOutputCurrent(); - inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); - inputs.feederVelocityRPM = feederEncoder.getVelocity(); - inputs.feederTempCelsius = feederMotor.getMotorTemperature(); - inputs.noteInIntake = !indexerBeamBreak.get(); - } - - @Override - public void setIntakeVoltage(double volts) { - intakeMotor.setVoltage(volts); - } - - @Override - public void setFeederVoltage(double volts) { - feederMotor.setVoltage(volts); - } - - @Override - public void setFeederVelocity(double velocityRPM) { - feederPID.setReference( - velocityRPM * FEEDER_GEAR_RATIO, - CANSparkBase.ControlType.kVelocity, - 0, - FEEDER_KS, - SparkPIDController.ArbFFUnits.kVoltage - ); - } - - @Override - public void configureFeederPID(double p, double i, double d) { - feederPID.setP(p); - feederPID.setI(i); - feederPID.setD(d); - } - - public void setIntakeBrakeMode(boolean isBrakeMode) { - intakeMotor.setIdleMode(isBrakeMode ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast); - } -} - diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java index 2f54331..51cb711 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java @@ -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); @@ -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