Merge branch 'zach/fix' of https://github.com/flamingchickens1540/rob…

…ot2024 into zach/fix

src/main/java/org/team1540/robot2024/Robot.java

@@ -31,8 +31,6 @@
 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);
 
     /**
      * This function is run when the robot is first started up and should be used for any
@@ -125,8 +123,6 @@ 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());
         // Update mechanism visualiser in sim
         if (Robot.isSimulation()) MechanismVisualiser.periodic();
 //        robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.alternating(Color.kBlueViolet, Color.kGreen));

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -148,6 +148,7 @@ private void configureButtonBindings() {
                 .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(() -> {

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);
                 }
             }

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;
+    }
 }

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;
     }
 
     /**

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

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