Adding disruptor to flywheel tuning

src/main/java/org/team2342/frc/Constants.java

@@ -139,8 +139,10 @@ public static final class ConductorConstants {
   }
 
   public static final class IndexerConstants {
-    public static final double RUN_VOLTAGE = 5;
+    public static final double RUN_VOLTAGE = 5.0;
+    public static final double DISRUPTOR_RUN_VOLTAGE = 5.0;
     public static final double RUN_CURRENT = 30.0;
+    public static final double MAX_CURRENT = 45.0;
 
     public static final MotorConfig INDEXER_MOTOR_CONFIG =
         new MotorConfig()
@@ -153,6 +155,18 @@ public static final class IndexerConstants {
     public static final DCMotorSim INDEXER_SIM =
         new DCMotorSim(
             LinearSystemId.createDCMotorSystem(INDEXER_SIM_MOTOR, 0.003, 1), INDEXER_SIM_MOTOR);
+
+    public static final MotorConfig DISRUPTOR_MOTOR_CONFIG =
+        new MotorConfig()
+            .withMotorInverted(true)
+            .withSupplyCurrentLimit(30.0)
+            .withStatorCurrentLimit(60.0)
+            .withIdleMode(MotorConfig.IdleMode.BRAKE);
+
+    public static final DCMotor DISRUPTOR_SIM_MOTOR = DCMotor.getNeoVortex(1);
+    public static final DCMotorSim DISRUPTOR_SIM =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(DISRUPTOR_SIM_MOTOR, 0.003, 1), DISRUPTOR_SIM_MOTOR);
   }
 
   public static final class IntakeConstants {
@@ -290,6 +304,8 @@ public static final class CANConstants {
     public static final int INTAKE_PIVOT_MOTOR_ID = 19;
     public static final int INTAKE_PIVOT_ENCODER_ID = 20;
 
+    public static final int DISRUPTOR_ID = 21;
+
     public static final int CANDLE_ID = 61;
     public static final int PDH_ID = 62;
   }

src/main/java/org/team2342/frc/RobotContainer.java

@@ -8,6 +8,7 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
@@ -44,6 +45,7 @@
 import org.team2342.frc.subsystems.drive.ModuleIO;
 import org.team2342.frc.subsystems.drive.ModuleIOSim;
 import org.team2342.frc.subsystems.drive.ModuleIOTalonFX;
+import org.team2342.frc.subsystems.indexer.Disruptor;
 import org.team2342.frc.subsystems.indexer.Indexer;
 import org.team2342.frc.subsystems.intake.Pivot;
 import org.team2342.frc.subsystems.intake.Wheels;
@@ -62,6 +64,7 @@
 import org.team2342.lib.leds.LedIOCANdle;
 import org.team2342.lib.motors.dumb.DumbMotorIO;
 import org.team2342.lib.motors.dumb.DumbMotorIOSim;
+import org.team2342.lib.motors.dumb.DumbMotorIOSparkFlex;
 import org.team2342.lib.motors.dumb.DumbMotorIOTalonFX;
 import org.team2342.lib.motors.dumb.DumbMotorIOTalonFXFOC;
 import org.team2342.lib.motors.smart.SmartMotorIO;
@@ -76,6 +79,7 @@ public class RobotContainer {
   @Getter private final Vision vision;
   @Getter private final Pivot pivot;
   @Getter private final Indexer indexer;
+  @Getter private final Disruptor disruptor;
   @Getter private final Kicker kicker;
   @Getter private final Wheels wheels;
   @Getter private final Flywheel flywheel;
@@ -143,6 +147,13 @@ public RobotContainer() {
             new Indexer(
                 new DumbMotorIOTalonFXFOC(
                     CANConstants.INDEXER_MOTOR_ID, IndexerConstants.INDEXER_MOTOR_CONFIG));
+
+        disruptor =
+            new Disruptor(
+                new DumbMotorIOSparkFlex(
+                    CANConstants.DISRUPTOR_ID,
+                    IndexerConstants.DISRUPTOR_MOTOR_CONFIG,
+                    MotorType.kBrushless));
         pivot =
             new Pivot(
                 new SmartMotorIOTalonFX(
@@ -200,6 +211,11 @@ public RobotContainer() {
                 new DumbMotorIOSim(
                     IndexerConstants.INDEXER_SIM_MOTOR, IndexerConstants.INDEXER_SIM));
 
+        disruptor =
+            new Disruptor(
+                new DumbMotorIOSim(
+                    IndexerConstants.DISRUPTOR_SIM_MOTOR, IndexerConstants.DISRUPTOR_SIM));
+
         wheels =
             new Wheels(
                 new DumbMotorIOSim(
@@ -243,6 +259,7 @@ public RobotContainer() {
                 new VisionIO() {},
                 new VisionIO() {});
         indexer = new Indexer(new DumbMotorIO() {});
+        disruptor = new Disruptor(new DumbMotorIO() {});
         wheels = new Wheels(new DumbMotorIO() {});
         pivot = new Pivot(new SmartMotorIO() {});
         flywheel = new Flywheel(new SmartMotorIO() {});
@@ -278,6 +295,7 @@ public RobotContainer() {
                     .alongWith(pivot.holdAngle(0))
                     .alongWith(wheels.in())
                     .alongWith(indexer.in())
+                    .alongWith(disruptor.in())
                     .alongWith(kicker.in())));
     if (Constants.TUNING) setupDevelopmentRoutines();
 
@@ -313,21 +331,15 @@ private void configureNamedCommands() {
         "autoShoot",
         conductor
             .runState(ConductorState.WARM_UP)
-            .withTimeout(1.0)
+            .withTimeout(2.0)
             .andThen(
                 conductor
                     .runState(ConductorState.TRACKED_FIRING)
-                    .alongWith(pivot.agitate())
+                    .alongWith(pivot.holdAngle(0))
                     .alongWith(wheels.in())
                     .alongWith(indexer.in())
-                    .alongWith(kicker.in())
-                    .withTimeout(4.0)
-                    .finallyDo(
-                        () -> {
-                          wheels.stop().schedule();
-                          indexer.stop().schedule();
-                          kicker.stop().schedule();
-                        })));
+                    .alongWith(disruptor.in())
+                    .alongWith(kicker.in())));
 
     NamedCommands.registerCommand(
         "autoIntake",
@@ -337,8 +349,10 @@ private void configureNamedCommands() {
         "stopAll",
         new InstantCommand(
             () -> {
+              pivot.stop();
               wheels.stop();
               indexer.stop();
+              disruptor.stop();
               kicker.stop();
             }));
   }
@@ -402,10 +416,10 @@ private void configureBindings() {
     driverController
         .rightTrigger()
         .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING))
-        .and(activeOrPassing)
         .and(readyToFire)
-        .whileTrue(Commands.parallel(indexer.pulseIn(), kicker.in()))
-        .onFalse(Commands.parallel(indexer.stop(), kicker.stop()));
+        .and(activeOrPassing)
+        .whileTrue(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))
+        .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), disruptor.stop()));
 
     // Firing during inactive period
     driverController
@@ -418,18 +432,19 @@ private void configureBindings() {
         .rightBumper()
         .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING))
         .and(readyToFire)
-        .whileTrue(Commands.parallel(indexer.pulseIn(), kicker.in()))
-        .onFalse(Commands.parallel(indexer.stop(), kicker.stop()));
+        .whileTrue(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))
+        .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), disruptor.stop()));
 
     // Operator override
     operatorController
         .rightTrigger()
-        .whileTrue(Commands.parallel(indexer.in(), kicker.in()))
+        .whileTrue(antiJamFeed())
         .onFalse(Commands.parallel(indexer.stop(), kicker.stop()));
     operatorController
         .rightBumper()
-        .whileTrue(Commands.parallel(indexer.out(), kicker.out(), wheels.out()))
-        .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), wheels.stop()));
+        .whileTrue(
+            Commands.parallel(indexer.out(), kicker.out(), wheels.out(), disruptor.reverse()))
+        .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), wheels.stop(), disruptor.stop()));
 
     // Turret Zero
     operatorController.back().onTrue(Commands.runOnce(() -> turret.zeroTurret()));
@@ -511,4 +526,22 @@ public void resetManual() {
   public static boolean withinBounds(double value, double bound1, double bound2) {
     return value <= Math.max(bound1, bound2) && value >= Math.min(bound1, bound2);
   }
+
+  private Command antiJamFeed() {
+    return Commands.sequence(
+            Commands.parallel(indexer.in(), disruptor.in(), kicker.in())
+                .until(() -> indexer.isJammed() || disruptor.isJammed()),
+            Commands.parallel(indexer.out(), disruptor.reverse(), kicker.out()).withTimeout(1.0),
+            Commands.parallel(indexer.in(), disruptor.in(), kicker.in()))
+        .repeatedly();
+  }
+
+  private Command pulsedAntiJamFeed() {
+    return Commands.sequence(
+            Commands.waitSeconds(1)
+                .andThen(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))
+                .until(() -> indexer.isJammed() || disruptor.isJammed()),
+            Commands.parallel(indexer.out(), disruptor.reverse(), kicker.out()).withTimeout(1.0))
+        .repeatedly();
+  }
 }

src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java

@@ -0,0 +1,57 @@
+// Copyright (c) 2026 Team 2342
+// https://github.com/FRCTeamPhoenix
+//
+// This source code is licensed under the MIT License.
+// See the LICENSE file in the root directory of this project.
+
+package org.team2342.frc.subsystems.indexer;
+
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+import org.team2342.frc.Constants.IndexerConstants;
+import org.team2342.lib.logging.ExecutionLogger;
+import org.team2342.lib.motors.dumb.DumbMotorIO;
+import org.team2342.lib.motors.dumb.DumbMotorIOInputsAutoLogged;
+
+public class Disruptor extends SubsystemBase {
+  private final DumbMotorIO motor;
+  private final DumbMotorIOInputsAutoLogged inputs = new DumbMotorIOInputsAutoLogged();
+
+  private final Alert motorAlert = new Alert("Disruptor Motor is diconnected", AlertType.kError);
+
+  public Disruptor(DumbMotorIO motor) {
+    this.motor = motor;
+    setName("Disruptor");
+
+    setDefaultCommand(run(() -> motor.runVoltage(0.0)));
+  }
+
+  @Override
+  public void periodic() {
+    motor.updateInputs(inputs);
+    Logger.processInputs("Disruptor", inputs);
+    motorAlert.set(!inputs.connected);
+    ExecutionLogger.log("Disruptor");
+  }
+
+  public Command in() {
+    return run(() -> motor.runVoltage(IndexerConstants.DISRUPTOR_RUN_VOLTAGE))
+        .withName("Disruptor Feed");
+  }
+
+  public Command reverse() {
+    return run(() -> motor.runVoltage(-IndexerConstants.DISRUPTOR_RUN_VOLTAGE))
+        .withName("Disruptor Reverse");
+  }
+
+  public Command stop() {
+    return run(() -> motor.runVoltage(0.0)).withName("Disruptor Stop");
+  }
+
+  public boolean isJammed() {
+    return inputs.currentAmps > IndexerConstants.DISRUPTOR_MOTOR_CONFIG.supplyLimit;
+  }
+}

src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java

@@ -57,4 +57,8 @@ public Command pulseIn() {
   public Command stop() {
     return runOnce(() -> motor.runVoltage(0.0)).withName("Indexer Stop");
   }
+
+  public boolean isJammed() {
+    return inputs.currentAmps > IndexerConstants.INDEXER_MOTOR_CONFIG.statorLimit;
+  }
 }