Independent intake rollers

src/main/java/frc/robot/Robot.java

@@ -58,9 +58,10 @@
 import frc.robot.subsystems.intake.IntakeArmIO;
 import frc.robot.subsystems.intake.IntakeArmIOReal;
 import frc.robot.subsystems.intake.IntakeArmIOSim;
-import frc.robot.subsystems.intake.IntakeRollerIO;
-import frc.robot.subsystems.intake.IntakeRollerIOSimTalonFX;
-import frc.robot.subsystems.intake.IntakeRollerIOTalonFX;
+import frc.robot.subsystems.intake.IntakeConstants.RollerConstants;
+import frc.robot.subsystems.intake.RollerIO;
+import frc.robot.subsystems.intake.RollerIOSimTalonFX;
+import frc.robot.subsystems.intake.RollerIOTalonFX;
 import frc.robot.subsystems.launcher.FlywheelIO;
 import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX;
 import frc.robot.subsystems.launcher.FlywheelIOTalonFX;
@@ -159,7 +160,11 @@ public Robot() {
                 new TurretIOSpark(),
                 new FlywheelIOTalonFX(),
                 new HoodIOSpark());
-        intake = new Intake(new IntakeRollerIOTalonFX(), new IntakeArmIOReal());
+        intake =
+            new Intake(
+                new RollerIOTalonFX(RollerConstants.upperRollerConfig),
+                new RollerIOTalonFX(RollerConstants.lowerRollerConfig),
+                new IntakeArmIOReal());
         // hopper = new Hopper(new HopperIOReal());
         feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark());
         SmartDashboard.putData(new Compressor(PneumaticsModuleType.REVPH));
@@ -197,7 +202,11 @@ public Robot() {
                 new FlywheelIOSimTalonFX(),
                 new HoodIOSimSpark());
         feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark());
-        intake = new Intake(new IntakeRollerIOSimTalonFX(), new IntakeArmIOSim());
+        intake =
+            new Intake(
+                new RollerIOSimTalonFX(RollerConstants.upperRollerConfig),
+                new RollerIOSimTalonFX(RollerConstants.lowerRollerConfig),
+                new IntakeArmIOSim());
         // hopper = new Hopper(new HopperIOSim());
         break;
 
@@ -232,7 +241,7 @@ public Robot() {
                 new TurretIO() {},
                 new FlywheelIO() {},
                 new HoodIO() {});
-        intake = new Intake(new IntakeRollerIO() {}, new IntakeArmIO() {});
+        intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {});
         // hopper = new Hopper(new HopperIO() {});
         feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {});
         break;

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

@@ -13,34 +13,41 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Intake extends SubsystemBase {
-  private final IntakeRollerIO intakeRollerIO;
+  private final RollerIO upperRollerIO;
+  private final RollerIO lowerRollerIO;
   private final IntakeArmIO intakeArmIO;
 
-  private final IntakeRollerIOInputsAutoLogged intakeRollerInputs =
-      new IntakeRollerIOInputsAutoLogged();
+  private final RollerIOInputsAutoLogged upperRollerInputs = new RollerIOInputsAutoLogged();
+  private final RollerIOInputsAutoLogged lowerRollerInputs = new RollerIOInputsAutoLogged();
   private final IntakeArmIOInputsAutoLogged intakeArmInputs = new IntakeArmIOInputsAutoLogged();
 
-  private final Alert disconnectedAlert;
+  private final Alert upperRollerDisconnectedAlert;
+  private final Alert lowerRollerDisconnectedAlert;
 
-  public Intake(IntakeRollerIO intakeRollerIO, IntakeArmIO intakeArmIO) {
-    this.intakeRollerIO = intakeRollerIO;
+  public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intakeArmIO) {
+    this.upperRollerIO = upperRollerIO;
+    this.lowerRollerIO = lowerRollerIO;
     this.intakeArmIO = intakeArmIO;
 
-    disconnectedAlert = new Alert("Disconnected intake motor", AlertType.kError);
+    upperRollerDisconnectedAlert = new Alert("Disconnected upper intake roller", AlertType.kError);
+    lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError);
   }
 
   @Override
   public void periodic() {
     long t0 = Constants.PROFILING_ENABLED ? System.nanoTime() : 0;
-    intakeRollerIO.updateInputs(intakeRollerInputs);
+    upperRollerIO.updateInputs(upperRollerInputs);
+    lowerRollerIO.updateInputs(lowerRollerInputs);
     intakeArmIO.updateInputs(intakeArmInputs);
     long t1 = Constants.PROFILING_ENABLED ? System.nanoTime() : 0;
 
-    Logger.processInputs("IntakeRoller", intakeRollerInputs);
+    Logger.processInputs("UpperRoller", upperRollerInputs);
+    Logger.processInputs("LowerRoller", lowerRollerInputs);
     Logger.processInputs("IntakeArm", intakeArmInputs);
     long t2 = Constants.PROFILING_ENABLED ? System.nanoTime() : 0;
 
-    disconnectedAlert.set(!intakeRollerInputs.connected);
+    upperRollerDisconnectedAlert.set(!upperRollerInputs.connected);
+    lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected);
 
     // Profiling output
     if (Constants.PROFILING_ENABLED) {
@@ -59,7 +66,8 @@ public void periodic() {
   }
 
   public void stop() {
-    intakeRollerIO.setOpenLoop(Volts.of(0.0));
+    upperRollerIO.setOpenLoop(Volts.of(0.0));
+    lowerRollerIO.setOpenLoop(Volts.of(0.0));
     intakeArmIO.retract();
   }
 
@@ -85,7 +93,12 @@ public Command getDeployCommand() {
             Commands.runOnce(this::deployArm, this),
             this.idle().withTimeout(0.5),
             Commands.startEnd(
-                () -> intakeRollerIO.setVelocity(MetersPerSecond.of(6.0)), () -> {}, this))
+                () -> {
+                  upperRollerIO.setVelocity(MetersPerSecond.of(6.0));
+                  lowerRollerIO.setVelocity(MetersPerSecond.of(6.0));
+                },
+                () -> {},
+                this))
         .withName("Intake");
   }
 
@@ -94,7 +107,12 @@ public Command getReverseCommand() {
             Commands.runOnce(this::deployArm, this),
             this.idle().withTimeout(0.5),
             Commands.startEnd(
-                () -> intakeRollerIO.setVelocity(MetersPerSecond.of(-4.0)), () -> {}, this))
+                () -> {
+                  upperRollerIO.setVelocity(MetersPerSecond.of(-4.0));
+                  lowerRollerIO.setVelocity(MetersPerSecond.of(-4.0));
+                },
+                () -> {},
+                this))
         .withName("Reverse");
   }
 }

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

@@ -2,15 +2,17 @@
 
 import static edu.wpi.first.units.Units.*;
 
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.Slot1Configs;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
+import frc.robot.Constants.CANBusPorts.CAN2;
 import frc.robot.Constants.MotorConstants.KrakenX60Constants;
 
 public class IntakeConstants {
-  public class IntakeRoller {
+  public static class RollerConstants {
     public static final Distance rollerRadius = Inches.of(0.85);
 
     // motor controller
@@ -27,5 +29,23 @@ public class IntakeRoller {
 
     // simulation
     public static final DCMotor gearbox = DCMotor.getKrakenX60(2);
+
+    // configs
+    public static final RollerConfig upperRollerConfig =
+        new RollerConfig(CAN2.intakeRollerUpper, CAN2.bus, true);
+    public static final RollerConfig lowerRollerConfig =
+        new RollerConfig(CAN2.intakeRollerLower, CAN2.bus, true);
+  }
+
+  public static class RollerConfig {
+    public final int port;
+    public final CANBus bus;
+    public final boolean inverted;
+
+    public RollerConfig(int port, CANBus bus, boolean inverted) {
+      this.port = port;
+      this.bus = bus;
+      this.inverted = inverted;
+    }
   }
 }