secondary intake motor (#72)

* Second intake motor

* spotless

Author: realjoshuau

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

@@ -14,6 +14,7 @@
 public final class IntakeConstants {
   /* CAN */
   public static final int kIntakeMotorID = 33;
+  public static final int kSecondaryIntakeMotorID = 999;
 
   public static final double kPassthroughIntakeVoltage = -8;
   public static final double kIntakeIntakeVoltage = 12;

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

@@ -9,6 +9,7 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.VelocityVoltage;
@@ -25,6 +26,9 @@ public class IntakeIOTalonFX implements IntakeIO {
       new MotionMagicVelocityVoltage(0).withSlot(0);
   private final VoltageOut intakeVoltageReq = new VoltageOut(0);
 
+  private final TalonFX secondaryIntakeMotor = new TalonFX(IntakeConstants.kSecondaryIntakeMotorID);
+  final Follower secondaryIntakeFollowReq = new Follower(intakeMotor.getDeviceID(), false);
+
   private final StatusSignal<Double> intakeMotorVoltage = intakeMotor.getMotorVoltage();
   private final StatusSignal<Double> intakeMotorVelocity = intakeMotor.getVelocity();
   private final StatusSignal<Double> intakeMotorStatorCurrent = intakeMotor.getStatorCurrent();
@@ -56,6 +60,11 @@ public IntakeIOTalonFX() {
     PhoenixUtil.checkErrorAndRetry(() -> intakeMotor.getConfigurator().refresh(motorConfig));
     TalonUtil.applyAndCheckConfiguration(intakeMotor, motorConfig);
 
+    var secondaryMotorConfig = IntakeConstants.intakeMotorConfig; // Same as intake motor.
+    PhoenixUtil.checkErrorAndRetry(
+        () -> secondaryIntakeMotor.getConfigurator().refresh(secondaryMotorConfig));
+    TalonUtil.applyAndCheckConfiguration(secondaryIntakeMotor, secondaryMotorConfig);
+
     var passthroughConfig = IntakeConstants.passthroughMotorConfig;
     PhoenixUtil.checkErrorAndRetry(
         () -> passthroughMotor.getConfigurator().refresh(passthroughConfig));
@@ -77,6 +86,7 @@ public IntakeIOTalonFX() {
         passthroughMotorReferenceSlope);
     intakeMotor.optimizeBusUtilization();
     passthroughMotor.optimizeBusUtilization();
+    secondaryIntakeMotor.setControl(secondaryIntakeFollowReq);
   }
 
   @Override