diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index ad1d207..d2298c3 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/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; } diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 96cbd16..496c5de 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/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(); + } } diff --git a/src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java b/src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java new file mode 100644 index 0000000..513cebf --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java index 697f4f0..85d67f8 100644 --- a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java +++ b/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; + } } diff --git a/src/main/java/org/team2342/lib b/src/main/java/org/team2342/lib index 90d6efc..21443d9 160000 --- a/src/main/java/org/team2342/lib +++ b/src/main/java/org/team2342/lib @@ -1 +1 @@ -Subproject commit 90d6efce9c4bedbf1580d897b97b19a1f9c3c73f +Subproject commit 21443d9d0c7103014452738f0a70c008759fa690