Intake/Indexer

src/main/java/frc/robot/Controls.java

@@ -41,6 +41,7 @@ public class Controls {
 
   public Controls(Subsystems subsystems) {
     s = subsystems;
+    configureIntake();
     // Configure the trigger bindings
     configureDrivebaseBindings();
   }
@@ -81,6 +82,12 @@ private double getDriveRotate() {
           .withRotationalDeadband(SWERVE_DEADBAND)
           .withDriveRequestType(DriveRequestType.Velocity);
 
+  private void configureIntake() {
+    if (s.intakeSubsystem != null) {
+      driverController.rightBumper().whileTrue(s.intakeSubsystem.startIntake());
+    }
+  }
+
   private void configureDrivebaseBindings() {
     if (s.drivebaseSubsystem == null) {
       // Stop running this method

src/main/java/frc/robot/Hardware.java

@@ -7,4 +7,7 @@
 public final class Hardware {
 
   public static final int PDH_ID = 1;
+
+  // TODO: better number when get robot
+  public static final int IntakeMotor = 0;
 }

src/main/java/frc/robot/Subsystems.java

@@ -1,19 +1,23 @@
 package frc.robot;
 
 import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED;
+import static frc.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED;
 
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import frc.robot.generated.AlphaTunerConstants;
 import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
+import frc.robot.subsystems.intake.IntakeSubsystem;
 
 public class Subsystems {
 
   // <SUBSYSTEM>_ENABLED constants go here
   public static class SubsystemConstants {
     public static final boolean DRIVEBASE_ENABLED = true;
+    public static final boolean INTAKE_ENABLED = true;
   }
 
   public final CommandSwerveDrivetrain drivebaseSubsystem;
+  public final IntakeSubsystem intakeSubsystem;
 
   public Subsystems(Mechanism2d mechanism2d) {
     // Initialize subsystems here (don't forget to check if they're enabled!)
@@ -33,5 +37,11 @@ public Subsystems(Mechanism2d mechanism2d) {
     } else {
       drivebaseSubsystem = null;
     }
+
+    if (INTAKE_ENABLED) {
+      intakeSubsystem = new IntakeSubsystem();
+    } else {
+      intakeSubsystem = null;
+    }
   }
 }

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -0,0 +1,72 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Hardware;
+
+public class IntakeSubsystem extends SubsystemBase {
+  private final VelocityTorqueCurrentFOC velocityRequest = new VelocityTorqueCurrentFOC(0);
+
+  private final TalonFX intakeMotor;
+
+  // TODO: set rps
+  private static final double TARGET_RPS = 40.0;
+
+  public IntakeSubsystem() {
+    // TODO: check with mech if motor on canivore or roborio
+    intakeMotor = new TalonFX(Hardware.IntakeMotor);
+    motorConfigs();
+  }
+
+  private void motorConfigs() {
+    var talonFXConfigs = new TalonFXConfiguration();
+    talonFXConfigs.MotorOutput.NeutralMode = NeutralModeValue.Coast; // KEEP THIS AT COAST
+    talonFXConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+    talonFXConfigs.CurrentLimits.StatorCurrentLimit = 80;
+    talonFXConfigs.CurrentLimits.SupplyCurrentLimit = 40;
+    talonFXConfigs.CurrentLimits.StatorCurrentLimitEnable = true;
+    talonFXConfigs.CurrentLimits.SupplyCurrentLimitEnable = true;
+
+    // TODO: set values
+    talonFXConfigs.Slot0.kP = 0.0;
+    talonFXConfigs.Slot0.kS = 0.0;
+    talonFXConfigs.Slot0.kA = 0.0;
+
+    // configurator
+    StatusCode status = StatusCode.StatusCodeNotInitialized;
+    for (int i = 0; i < 5; ++i) {
+      status = intakeMotor.getConfigurator().apply(talonFXConfigs);
+      if (status.isOK()) break;
+    }
+    if (!status.isOK()) {
+      DriverStation.reportError("Failed to apply IntakeMotor configs: " + status, false);
+    }
+  }
+
+  public Command startIntake() {
+    return startEnd(
+            () -> {
+              runIntake(TARGET_RPS);
+            },
+            () -> {
+              stopIntake();
+            })
+        .withName("Run Intake");
+  }
+
+  private void runIntake(double velocity) {
+    intakeMotor.setControl(velocityRequest.withVelocity(velocity));
+  }
+
+  private void stopIntake() {
+    intakeMotor.stopMotor();
+  }
+}