Basic indexer implemented to help with Shooter testing

src/main/java/frc/robot/Constants.java

@@ -74,5 +74,9 @@ public static final class Ports {
 
     public static final int kFlywheelLeft = 0;
     public static final int kFlywheelRight = 0;
+
+    public static final int kIndexerMotor = 0;
+    public static final int kIndexerBeamBreakOne = 0;
+    public static final int kIndexerBeamBreakTwo = 0;
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -25,6 +25,9 @@
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.indexer.IndexerIOFalcon;
+import frc.robot.subsystems.indexer.IndexerIOSim;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIOKraken;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
@@ -43,6 +46,7 @@ public class RobotContainer {
   // Subsystems
   private Drive m_drive;
   private Shooter m_shooter;
+  private Indexer m_indexer;
 
   // Controllers
   private DriverControls m_driverControls;
@@ -73,6 +77,11 @@ private void configureSubsystems() {
               new FlywheelIOKraken(Ports.kFlywheelLeft, Ports.kFlywheelRight),
               ShooterConstants.kPivotController,
               ShooterConstants.kFlywheelController);
+      m_indexer =
+          new Indexer(
+            new IndexerIOFalcon(Ports.kIndexerMotor,
+                                Ports.kIndexerBeamBreakOne,
+                                Ports.kIndexerBeamBreakTwo));
     } else {
       m_drive =
           new Drive(
@@ -87,11 +96,12 @@ private void configureSubsystems() {
               new FlywheelIOSim(),
               ShooterConstants.kPivotController,
               ShooterConstants.kFlywheelController);
+      m_indexer = new Indexer(new IndexerIOSim());
     }
 
     m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
 
-    m_robotState = RobotState.startInstance(m_drive, m_shooter);
+    m_robotState = RobotState.startInstance(m_drive, m_shooter, m_indexer);
   }
 
   private void configureControllers() {
@@ -120,6 +130,12 @@ private void configureButtonBindings() {
                   m_shooter.setFlywheelVelocity(0.0, 0.0);
                   m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
                 }));
+
+    m_driverControls.testIndexer().onTrue(
+      m_indexer.runIndexer(6.0)
+    ).onFalse(
+      m_indexer.runIndexer(0.0)
+    );
   }
 
   public void updateRobotState() {

src/main/java/frc/robot/RobotState.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.shooter.Shooter;
 
 public class RobotState {
@@ -15,12 +16,13 @@ public class RobotState {
   // subsystems
   private Drive m_drive;
   private Shooter m_shooter;
+  private Indexer m_indexer;
 
   // mechanism
   private final boolean kMechanismEnabled = true;
   MechanismLigament2d m_shooterPivotMech;
 
-  private RobotState(Drive drive, Shooter shooter) {
+  private RobotState(Drive drive, Shooter shooter, Indexer indexer) {
     // subsystems
     m_drive = drive;
     m_shooter = shooter;
@@ -41,9 +43,9 @@ private RobotState(Drive drive, Shooter shooter) {
     SmartDashboard.putData("Mech2d", m_mechanism);
   }
 
-  public static RobotState startInstance(Drive drive, Shooter shooter) {
+  public static RobotState startInstance(Drive drive, Shooter shooter, Indexer indexer) {
     if (instance == null) {
-      instance = new RobotState(drive, shooter);
+      instance = new RobotState(drive, shooter, indexer);
     } else {
       throw new IllegalStateException("RobotState instance already started");
     }

src/main/java/frc/robot/oi/DriverControls.java

@@ -10,4 +10,6 @@ public interface DriverControls {
   public double getDriveRotation();
 
   public Trigger testShooter();
+
+  public Trigger testIndexer();
 }

src/main/java/frc/robot/oi/DriverControlsPS5.java

@@ -29,4 +29,9 @@ public double getDriveRotation() {
   public Trigger testShooter() {
     return m_controller.L2();
   }
+
+  @Override
+  public Trigger testIndexer() {
+    return m_controller.R2();
+  }
 }

src/main/java/frc/robot/oi/DriverControlsXbox.java

@@ -27,6 +27,11 @@ public double getDriveRotation() {
 
   @Override
   public Trigger testShooter() {
-    return m_controller.button(7);
+    return m_controller.button(1);
+  }
+
+  @Override
+  public Trigger testIndexer() {
+    return m_controller.button(2);
   }
 }

src/main/java/frc/robot/subsystems/indexer/Indexer.java

@@ -0,0 +1,17 @@
+package frc.robot.subsystems.indexer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Indexer extends SubsystemBase {
+    private IndexerIO m_io;
+
+    public Indexer(IndexerIO io) {
+        m_io = io;
+    }
+
+    public Command runIndexer(double voltage) {
+        return Commands.runOnce(() -> m_io.setVoltage(voltage));
+    }
+}

src/main/java/frc/robot/subsystems/indexer/IndexerIO.java

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.indexer;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import frc.lib.advantagekit.LoggedIO;
+
+public interface IndexerIO extends LoggedIO<IndexerIO.IndexerInputs> {
+    @AutoLog
+    public static class IndexerInputs {
+        public double curVelocity;
+        public double curVoltage;
+        public double curAmps;
+        public boolean beamBreakOneBroken;
+        public boolean beamBreakTwoBroken;
+    }
+
+    public void setVoltage(double voltage);
+
+}

src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java

@@ -0,0 +1,48 @@
+package frc.robot.subsystems.indexer;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class IndexerIOFalcon implements IndexerIO {
+    private TalonFX m_indexerMotor;
+    private DigitalInput m_beamBreakOne;
+    private DigitalInput m_beamBreakTwo;
+
+    private StatusSignal<Double> m_curVelocity;
+    private StatusSignal<Double> m_curVoltage;
+    private StatusSignal<Double> m_curAmps;
+
+    public IndexerIOFalcon(int motorPort, int beamBreakOnePort, int beamBreakTwoPort) {
+        m_indexerMotor = new TalonFX(motorPort);
+        m_beamBreakOne = new DigitalInput(beamBreakOnePort);
+        m_beamBreakTwo = new DigitalInput(beamBreakTwoPort);
+
+        m_curVelocity = m_indexerMotor.getVelocity();
+        m_curVoltage = m_indexerMotor.getMotorVoltage();
+        m_curAmps = m_indexerMotor.getSupplyCurrent();
+    }
+
+    @Override
+    public void updateInputs(IndexerInputs inputs) {
+        BaseStatusSignal.refreshAll(
+            m_curVelocity,
+            m_curVoltage,
+            m_curAmps
+        );
+
+        inputs.curVelocity = Units.rotationsToRadians(m_curVelocity.getValue());
+        inputs.curVoltage = m_curVoltage.getValue();
+        inputs.curAmps = m_curAmps.getValue();
+        inputs.beamBreakOneBroken = m_beamBreakOne.get();
+        inputs.beamBreakTwoBroken = m_beamBreakTwo.get();
+    }
+
+    @Override
+    public void setVoltage(double voltage) {
+        m_indexerMotor.setVoltage(voltage);
+    }
+}

src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java

@@ -0,0 +1,43 @@
+package frc.robot.subsystems.indexer;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import frc.robot.Constants.Ports;
+
+public class IndexerIOSim implements IndexerIO {
+    private DCMotorSim m_motorSim;
+    private double m_curVoltage;
+    private DIOSim m_beamBreakOne;
+    private DIOSim m_beamBreakTwo;
+
+    public IndexerIOSim() {
+        double simGearing = 1.0;
+        double simJkGMetersSquared = 1.0;
+        m_motorSim = new DCMotorSim(
+            DCMotor.getFalcon500(1),
+            simGearing,
+            simJkGMetersSquared
+        );
+
+        m_beamBreakOne = new DIOSim(Ports.kIndexerBeamBreakOne);
+        m_beamBreakTwo = new DIOSim(Ports.kIndexerBeamBreakTwo);
+    }
+
+    @Override
+    public void updateInputs(IndexerInputs inputs) {
+        m_motorSim.update(0.02);
+
+        inputs.curVoltage = m_curVoltage;
+        inputs.curVelocity = m_motorSim.getAngularVelocityRadPerSec();
+        inputs.curAmps = m_motorSim.getCurrentDrawAmps();
+        inputs.beamBreakOneBroken = m_beamBreakOne.getValue();
+        inputs.beamBreakTwoBroken = m_beamBreakTwo.getValue();
+    }
+
+    @Override
+    public void setVoltage(double voltage) {
+        m_motorSim.setInputVoltage(voltage);
+        m_curVoltage = voltage;
+    }
+}