Indexer

2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/Indexer/Indexer.java

@@ -0,0 +1,70 @@
+package frc.robot.subsystems.Indexer;
+
+import org.littletonrobotics.junction.Logger;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+import frc.robot.subsystems.Indexer.IndexerIO.IndexerIOInputs;
+
+public class Indexer extends SubsystemBase {
+    private final IndexerIO io;
+  IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged();
+
+  public Indexer(IndexerIO io) {
+    this.io = io;
+  }
+
+  public void periodic() {
+    io.updateInputs(inputs);
+
+    Logger.processInputs("Indexer", inputs);
+
+    io.beamBreakPeriodic();
+  }
+
+  @AutoLogOutput(key = "Indexer/IndexerAppliedVoltage")
+  public double getVoltage() {
+    return inputs.appliedVoltage;
+  }
+
+  public Command applyVoltage(double voltage) {
+    return Commands.run(
+        () -> {
+          io.setVoltage(voltage);
+        });
+  }
+
+  public Command stop() {
+    return Commands.run(
+        () -> {
+          io.setVoltage(0);
+        });
+  }
+
+  public Command setVelocity(double velocity) {
+    return Commands.run(
+        () -> {
+          io.setVelocity(velocity);
+        }
+        );
+  }
+  public Command goToPose(double pose){
+    return Commands.run(
+        () -> {
+          io.goToPose(pose);
+        });
+  }
+  public void setBrake(boolean brake){
+    io.setBrake(brake);
+  }
+  public Command indexerTillBeamBreak(){
+	return Commands.sequence(
+	new InstantCommand(()-> io.runVelocity(IndexerConstants.intakeVelocity)),
+	new WaitUntilCommand(()-> io.noteInIndexer()),
+	new InstantCommand(() -> io.runVelocity(0)));
+}
+}

2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/Indexer/IndexerConstants.java

@@ -0,0 +1,33 @@
+package frc.robot.subsystems.Indexer;
+
+public class IndexerConstants {
+    public static final int indexerMotorID = 0;
+    public static final String indexerMotorCANBus = "rio";
+    public static final double realkP = 0.0;
+    public static final double realkI = 0.0;
+    public static final double realkD = 0.0;
+    public static final double realkS = 0.0;
+    public static final double realkV = 0.0;
+
+    public static final double gearRatio = 1.5;
+    public static final double indexerMOI = 0.04;
+
+    public static final boolean EnableCurrentLimit = true;
+    public static final int ContinousCurrentLimit = 50;
+    public static final int PeakCurrentLimit = 50;
+    public static final double PeakCurrentDuration = 0.1;
+
+    public static final double simkP = 0.0;
+    public static final double simkI = 0.0;
+    public static final double simkD = 0.0;
+    public static final double simkS = 0.0;
+    public static final double simkV = 0.0;
+
+    public static final double simVelocityConstant = 0.2;
+
+    public static final int beamBreakPort = 1;
+    public static final double beamBreakDebounce = 0.05;
+    public static final double intakeVelocity = 20;
+
+
+}

2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/Indexer/IndexerIO.java

@@ -0,0 +1,31 @@
+package frc.robot.subsystems.Indexer;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IndexerIO {
+
+    @AutoLog
+    public static class IndexerIOInputs {
+        public double velocityRadsPerSec = 0.0;
+        public double appliedVoltage = 0.0;
+        public double speedSetpoint = 0.0;
+        public double currentAmps = 0.0;
+        public double tempCelcius = 0.0;
+    }
+    public default void updateInputs(IndexerIOInputs inputs) {}
+
+    public default void setVoltage(double voltage) {}
+
+    public default void goToPose(double position) {}
+
+    public default void setVelocity(double velocity) {}
+
+    public default void setBrake(boolean brake) {}
+
+    public default void beamBreakPeriodic() {}
+
+    public default boolean noteInIndexer() {
+        return false;
+    }
+    public default void runVelocity(double velocity) {}
+}

2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.Indexer;
+
+import static frc.robot.subsystems.Indexer.IndexerConstants.*;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+
+public class IndexerIOSim implements IndexerIO {
+    // private final driveSim =
+    // new DCMotorSim(
+    //     .createDCMotorSystem(
+    //         DRIVE_GEARBOX, constants.DriveInertia, constants.DriveMotorGearRatio),
+    //     DRIVE_GEARBOX);
+    private static final DCMotor INDEXER_GEARBOX = DCMotor.getKrakenX60Foc(1);
+    private final DCMotorSim indexerMotorSim = new DCMotorSim(
+        LinearSystemId.createDCMotorSystem(INDEXER_GEARBOX, indexerMOI, gearRatio),
+        INDEXER_GEARBOX); 
+    private PIDController controller = new PIDController(0, indexerMotorID, indexerMotorID);
+    private SimpleMotorFeedforward ff;
+    private double appliedVoltage;
+    private double desiredSpeed;
+
+    public IndexerIOSim() {
+        controller.setPID(simkP, simkI, simkD);
+        ff = new SimpleMotorFeedforward(simkS, simkV);
+    } 
+
+    @Override 
+    public void updateInputs(IndexerIOInputs inputs) {
+        indexerMotorSim.update(0.02);
+        inputs.velocityRadsPerSec = indexerMotorSim.getAngularAccelerationRadPerSecSq();
+        inputs.appliedVoltage = appliedVoltage;
+        inputs.currentAmps = indexerMotorSim.getCurrentDrawAmps();
+        inputs.tempCelcius = 60;
+        inputs.speedSetpoint = desiredSpeed;
+    }
+    @Override 
+    public void setVoltage(double volts) {
+        appliedVoltage = volts;
+        indexerMotorSim.setInputVoltage(volts);
+    }
+    @Override 
+    public void goToPose(double pose) {
+        double volts = controller.calculate(indexerMotorSim.getAngularPositionRad(), pose) + ff.calculate(simVelocityConstant);
+        indexerMotorSim.setInputVoltage(volts);
+    }
+    @Override 
+    public void setVelocity(double velocity) {
+        double volts = controller.calculate(indexerMotorSim.getAngularVelocityRadPerSec(), velocity);
+    }
+    @Override
+    public void beamBreakPeriodic() {}
+
+    @Override
+    public boolean noteInIndexer() {
+        return false;
+    }
+}

2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/Indexer/IndexerIOTalonFX.java

@@ -0,0 +1,108 @@
+package frc.robot.subsystems.Indexer;
+
+import static frc.robot.subsystems.Indexer.IndexerConstants.*;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Temperature;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Timer;
+
+public class IndexerIOTalonFX implements IndexerIO {
+    private TalonFX indexerMotor;
+    TalonFXConfiguration indexerConfig;
+
+    private final StatusSignal<AngularVelocity> velocityRadsPerSec;
+    private final StatusSignal<Voltage> appliedVoltage;
+    private double desiredSpeedSetpoint;
+    private final StatusSignal<Current> currentAmps;
+    private final StatusSignal<Temperature> tempCelcius;
+    private final PositionVoltage indexerPositionVoltage = new PositionVoltage(0). withSlot(0);
+    private final VelocityVoltage indexerVelocityVoltage = new VelocityVoltage(0). withSlot(0);
+
+    DigitalInput beamBreak = new DigitalInput(beamBreakPort);
+    Timer beamBreakTimer = new Timer();
+
+    public IndexerIOTalonFX() {
+        indexerMotor = new TalonFX(indexerMotorID, indexerMotorCANBus);
+        indexerConfig = new TalonFXConfiguration();
+        indexerConfig.Slot0.kP = realkP;
+        indexerConfig.Slot0.kP = realkI;
+        indexerConfig.Slot0.kP = realkD;
+        indexerConfig.Slot0.kP = realkS;
+        indexerConfig.Slot0.kP = realkV;
+        indexerConfig.CurrentLimits.SupplyCurrentLimitEnable = EnableCurrentLimit;
+        indexerConfig.CurrentLimits.SupplyCurrentLimit = ContinousCurrentLimit;
+        indexerConfig.CurrentLimits.SupplyCurrentLowerLimit = PeakCurrentLimit;
+        indexerConfig.CurrentLimits.SupplyCurrentLowerTime = PeakCurrentDuration;
+
+        appliedVoltage = indexerMotor.getSupplyVoltage();
+        velocityRadsPerSec = indexerMotor.getVelocity();
+        tempCelcius = indexerMotor.getDeviceTemp();
+        currentAmps = indexerMotor.getSupplyCurrent();
+
+        BaseStatusSignal.setUpdateFrequencyForAll(
+            50.0, appliedVoltage, velocityRadsPerSec, tempCelcius, currentAmps);
+
+        indexerMotor.optimizeBusUtilization();
+        indexerMotor.getConfigurator().apply(indexerConfig);
+
+        beamBreakTimer.start();
+    }
+
+    @Override
+    public void updateInputs(IndexerIOInputs inputs) {
+        BaseStatusSignal.refreshAll(appliedVoltage, velocityRadsPerSec, tempCelcius, currentAmps);
+
+        inputs.speedSetpoint = desiredSpeedSetpoint;
+        inputs.appliedVoltage = appliedVoltage.getValueAsDouble();
+        inputs.tempCelcius = tempCelcius.getValueAsDouble();
+        inputs.currentAmps = currentAmps.getValueAsDouble();
+        inputs.velocityRadsPerSec = velocityRadsPerSec.getValueAsDouble();
+    }
+
+    @Override
+    public void setVoltage(double voltage) {
+        indexerMotor.setVoltage(voltage);
+    }
+
+    @Override
+    public void setBrake(boolean brake) {
+        if (brake) {
+             indexerMotor.setNeutralMode(NeutralModeValue.Brake);   
+        } else {
+            indexerMotor.setNeutralMode(NeutralModeValue.Coast);
+        }
+    }
+
+    @Override
+    public void goToPose(double pose) {
+        indexerMotor.setControl(indexerPositionVoltage.withPosition(Units.radiansToRotations(pose)));
+    }
+    public void setVelocity(double velocity) {
+        indexerMotor.setControl(indexerVelocityVoltage.withVelocity(Units.radiansPerSecondToRotationsPerMinute(velocity)/60));
+    }
+
+    @Override
+    public boolean noteInIndexer() {
+        return beamBreakTimer.get()>=IndexerConstants.beamBreakDebounce;   
+    }
+
+    @Override 
+    public void beamBreakPeriodic() {
+        if(beamBreak.get()){
+            beamBreakTimer.restart();
+        }
+    }
+
+}