Start adding centering motor to intake subsystem

Author: jonahsnider

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

@@ -15,7 +15,9 @@ public class Hardware {
       new TalonFX(RobotConfig.get().queuer().motorID(), RobotConfig.get().queuer().canBusName());
 
   public final TalonFX intake =
-      new TalonFX(RobotConfig.get().intake().motorID(), RobotConfig.get().intake().canBusName());
+      new TalonFX(
+          RobotConfig.get().intake().mainMotorID(),
+          RobotConfig.get().intake().mainMotorCanBusName());
 
   public final TalonFX armLeft =
       new TalonFX(RobotConfig.get().arm().leftMotorID(), RobotConfig.get().arm().canBusName());

src/main/java/frc/robot/config/CompConfig.java

@@ -66,13 +66,18 @@ class CompConfig {
               999,
               CANIVORE_NAME,
               999,
+              999,
               new TalonFXConfiguration()
                   .withClosedLoopRamps(CLOSED_LOOP_RAMP)
                   .withOpenLoopRamps(OPEN_LOOP_RAMP)
                   .withCurrentLimits(
                       new CurrentLimitsConfigs()
                           .withStatorCurrentLimit(20)
                           .withSupplyCurrentLimit(25)),
+              centeringMotor -> {
+                centeringMotor.setSmartCurrentLimit(20);
+                centeringMotor.burnFlash();
+              },
               new Debouncer(3.0 * 0.02)),
           new ArmConfig(
               CANIVORE_NAME,

src/main/java/frc/robot/config/RobotConfig.java

@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.mechanisms.swerve.utility.PhoenixPIDController;
+import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import java.util.function.Consumer;
@@ -35,10 +36,12 @@ public record ShooterConfig(
       Consumer<InterpolatingDoubleTreeMap> speakerDistanceToRpm) {}
 
   public record IntakeConfig(
-      int motorID,
-      String canBusName,
+      int mainMotorID,
+      String mainMotorCanBusName,
+      int centeringMotorID,
       int sensorID,
-      TalonFXConfiguration motorConfig,
+      TalonFXConfiguration mainMotorConfig,
+      Consumer<CANSparkMax> centeringMotorConfig,
       Debouncer debouncer) {}
 
   public record ArmConfig(

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

@@ -9,18 +9,22 @@
 import frc.robot.util.state_machines.StateMachine;
 
 public class IntakeSubsystem extends StateMachine<IntakeState> {
-  private final TalonFX motor;
+  private final TalonFX mainMotor;
   private final DigitalInput sensor;
   private boolean sensorHasNote = false;
   private boolean debouncedSensorHasNote = false;
   private final Debouncer debouncer = RobotConfig.get().intake().debouncer();
 
-  public IntakeSubsystem(TalonFX motor, DigitalInput sensor) {
+  public IntakeSubsystem(TalonFX mainMotor, DigitalInput sensor) {
     super(SubsystemPriority.INTAKE, IntakeState.IDLE);
 
     this.sensor = sensor;
-    this.motor = motor;
-    motor.getConfigurator().apply(RobotConfig.get().intake().motorConfig());
+    this.mainMotor = mainMotor;
+
+    mainMotor.getConfigurator().apply(RobotConfig.get().intake().mainMotorConfig());
+
+    // TODO: Uncomment this once the centeringMotor code is written
+    // RobotConfig.get().intake().centeringMotorConfig().accept(centeringMotor);
   }
 
   public void setState(IntakeState newState) {
@@ -45,18 +49,18 @@ public boolean hasNote() {
   @Override
   protected void afterTransition(IntakeState newState) {
     switch (newState) {
-      case IDLE -> motor.disable();
-      case INTAKING -> motor.setVoltage(0); // around 10
-      case OUTTAKING -> motor.setVoltage(0); // around -6
+      case IDLE -> mainMotor.disable();
+      case INTAKING -> mainMotor.setVoltage(0); // around 10
+      case OUTTAKING -> mainMotor.setVoltage(0); // around -6
     }
   }
 
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();
-    DogLog.log("Intake/StatorCurrent", motor.getStatorCurrent().getValueAsDouble());
-    DogLog.log("Intake/SupplyCurrent", motor.getSupplyCurrent().getValueAsDouble());
-    DogLog.log("Intake/AppliedVoltage", motor.getMotorVoltage().getValueAsDouble());
+    DogLog.log("Intake/StatorCurrent", mainMotor.getStatorCurrent().getValueAsDouble());
+    DogLog.log("Intake/SupplyCurrent", mainMotor.getSupplyCurrent().getValueAsDouble());
+    DogLog.log("Intake/AppliedVoltage", mainMotor.getMotorVoltage().getValueAsDouble());
     DogLog.log("Intake/RawSensor", sensorHasNote);
     DogLog.log("Intake/DebouncedSensor", hasNote());
   }