From fe998df47f56e9c87611e44d8d2c6c3602f86d3c Mon Sep 17 00:00:00 2001 From: abi-appusamy9932 Date: Sun, 22 Mar 2026 16:29:13 -0400 Subject: [PATCH 1/4] add disruptor --- src/main/java/org/team2342/frc/Constants.java | 3 +- .../frc/subsystems/indexer/Disruptor.java | 44 +++++++++++++++++++ 2 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 0210d36..63c987d 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -139,7 +139,8 @@ 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 MotorConfig INDEXER_MOTOR_CONFIG = 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..e1c61e8 --- /dev/null +++ b/src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java @@ -0,0 +1,44 @@ +package org.team2342.frc.subsystems.indexer; + +import org.team2342.lib.logging.ExecutionLogger; +import org.team2342.lib.motors.dumb.DumbMotorIO; +import org.team2342.lib.motors.dumb.DumbMotorIOInputsAutoLogged; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.Command; +import org.littletonrobotics.junction.Logger; +import org.team2342.frc.Constants.IndexerConstants; + +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"); + } +} From d7a0ad8317c973f3975673636eddbf763fc19c8b Mon Sep 17 00:00:00 2001 From: abi-appusamy9932 Date: Mon, 23 Mar 2026 20:10:16 -0400 Subject: [PATCH 2/4] update robot container --- .../java/org/team2342/frc/RobotContainer.java | 57 ++++++++++++------- 1 file changed, 38 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index c100872..d419142 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -8,6 +8,8 @@ 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; @@ -45,6 +47,7 @@ import org.team2342.frc.subsystems.drive.ModuleIOSim; import org.team2342.frc.subsystems.drive.ModuleIOTalonFX; import org.team2342.frc.subsystems.indexer.Indexer; +import org.team2342.frc.subsystems.indexer.Disruptor; import org.team2342.frc.subsystems.intake.Pivot; import org.team2342.frc.subsystems.intake.Wheels; import org.team2342.frc.subsystems.leds.LEDSubsystem; @@ -62,6 +65,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 +80,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; @@ -142,6 +147,11 @@ public RobotContainer() { new Indexer( new DumbMotorIOTalonFXFOC( CANConstants.INDEXER_MOTOR_ID, IndexerConstants.INDEXER_MOTOR_CONFIG)); + + disruptor = + new Disruptor( + new DumbMotorIOSparkFlex( + 0, IndexerConstants.DISRUPTOR_MOTOR_CONFIG, MotorType.kBrushless)); //TODO: can id pivot = new Pivot( new SmartMotorIOTalonFX( @@ -199,6 +209,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( @@ -242,6 +257,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() {}); @@ -277,6 +293,7 @@ public RobotContainer() { .alongWith(pivot.holdAngle(0)) .alongWith(wheels.in()) .alongWith(indexer.in()) + .alongWith(disruptor.in()) .alongWith(kicker.in()))); if (Constants.TUNING) setupDevelopmentRoutines(); @@ -311,21 +328,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", @@ -335,8 +346,10 @@ private void configureNamedCommands() { "stopAll", new InstantCommand( () -> { + pivot.stop(); wheels.stop(); indexer.stop(); + disruptor.stop(); kicker.stop(); })); } @@ -402,7 +415,7 @@ private void configureBindings() { .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) .and(activeOrPassing) .whileTrue( - Commands.waitSeconds(1).andThen(Commands.parallel(indexer.pulseIn(), kicker.in()))) + Commands.waitSeconds(1).andThen(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))) .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); // Firing during inactive period @@ -416,28 +429,34 @@ private void configureBindings() { .rightBumper() .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) .whileTrue( - Commands.waitSeconds(1).andThen(Commands.parallel(indexer.pulseIn(), kicker.in()))) + Commands.waitSeconds(1).andThen(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))) .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); // Operator override operatorController .rightTrigger() - .whileTrue(Commands.parallel(indexer.in(), kicker.in())) + .whileTrue(Commands.parallel(indexer.in(), kicker.in(), disruptor.in())) .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())); + operatorController + .back() + .onTrue(Commands.runOnce(() -> turret.zeroTurret())); // Manual Mode operatorController .start() .toggleOnTrue(conductor.forceManual().alongWith(Commands.runOnce(this::resetManual))); - operatorController.povUp().whileTrue(Commands.run(() -> flywheelManual += 0.1)); - operatorController.povDown().whileTrue(Commands.run(() -> flywheelManual -= 0.1)); + operatorController + .povUp() + .whileTrue(Commands.run(() -> flywheelManual += 0.1)); + operatorController + .povDown() + .whileTrue(Commands.run(() -> flywheelManual -= 0.1)); operatorController .povLeft() .whileTrue(Commands.run(() -> turretManual -= Units.degreesToRadians(0.75))); From 447f2870e9e621d57f0f72a03d517b8e8f81d2bc Mon Sep 17 00:00:00 2001 From: abi-appusamy9932 Date: Mon, 23 Mar 2026 21:04:07 -0400 Subject: [PATCH 3/4] anti jamming --- src/main/java/org/team2342/frc/Constants.java | 13 +++ .../java/org/team2342/frc/RobotContainer.java | 77 ++++++++++++----- .../frc/subsystems/indexer/Disruptor.java | 83 +++++++++++-------- .../frc/subsystems/indexer/Indexer.java | 4 + 4 files changed, 121 insertions(+), 56 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 63c987d..190caa3 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -142,6 +142,7 @@ public static final class IndexerConstants { 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() @@ -154,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(false) + .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 { diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index d419142..a997395 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -9,7 +9,6 @@ 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; @@ -46,8 +45,8 @@ 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.Indexer; 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; import org.team2342.frc.subsystems.leds.LEDSubsystem; @@ -147,11 +146,13 @@ public RobotContainer() { new Indexer( new DumbMotorIOTalonFXFOC( CANConstants.INDEXER_MOTOR_ID, IndexerConstants.INDEXER_MOTOR_CONFIG)); - - disruptor = + + disruptor = new Disruptor( new DumbMotorIOSparkFlex( - 0, IndexerConstants.DISRUPTOR_MOTOR_CONFIG, MotorType.kBrushless)); //TODO: can id + 0, + IndexerConstants.DISRUPTOR_MOTOR_CONFIG, + MotorType.kBrushless)); // TODO: can id pivot = new Pivot( new SmartMotorIOTalonFX( @@ -414,8 +415,7 @@ private void configureBindings() { .rightTrigger() .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) .and(activeOrPassing) - .whileTrue( - Commands.waitSeconds(1).andThen(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))) + .whileTrue(pulsedAntiJamFeed()) .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); // Firing during inactive period @@ -428,35 +428,29 @@ private void configureBindings() { driverController .rightBumper() .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) - .whileTrue( - Commands.waitSeconds(1).andThen(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))) + .whileTrue(pulsedAntiJamFeed()) .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); // Operator override operatorController .rightTrigger() - .whileTrue(Commands.parallel(indexer.in(), kicker.in(), disruptor.in())) + .whileTrue(antiJamFeed()) .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); operatorController .rightBumper() - .whileTrue(Commands.parallel(indexer.out(), kicker.out(), wheels.out(), disruptor.reverse())) + .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())); + operatorController.back().onTrue(Commands.runOnce(() -> turret.zeroTurret())); // Manual Mode operatorController .start() .toggleOnTrue(conductor.forceManual().alongWith(Commands.runOnce(this::resetManual))); - operatorController - .povUp() - .whileTrue(Commands.run(() -> flywheelManual += 0.1)); - operatorController - .povDown() - .whileTrue(Commands.run(() -> flywheelManual -= 0.1)); + operatorController.povUp().whileTrue(Commands.run(() -> flywheelManual += 0.1)); + operatorController.povDown().whileTrue(Commands.run(() -> flywheelManual -= 0.1)); operatorController .povLeft() .whileTrue(Commands.run(() -> turretManual -= Units.degreesToRadians(0.75))); @@ -477,6 +471,8 @@ private void configureBindings() { shiftAboutToEnd .and(RobotModeTriggers.teleop()) .onTrue(driverController.rumble(RumbleType.kRightRumble, 1.0).withTimeout(0.25)); + + } public Command getAutonomousCommand() { @@ -528,4 +524,43 @@ 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 index e1c61e8..067a303 100644 --- a/src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java +++ b/src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java @@ -1,44 +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 org.team2342.lib.logging.ExecutionLogger; -import org.team2342.lib.motors.dumb.DumbMotorIO; -import org.team2342.lib.motors.dumb.DumbMotorIOInputsAutoLogged; -import edu.wpi.first.wpilibj2.command.SubsystemBase; 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 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.MAX_CURRENT; + } } 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..23fed19 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.MAX_CURRENT; + } } From 3832cc52f9bf94f06ea62e9c60797bc264fba62b Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Thu, 26 Mar 2026 18:38:36 -0400 Subject: [PATCH 4/4] tuning disruptor + removing anti jam --- src/main/java/org/team2342/frc/Constants.java | 6 +- .../java/org/team2342/frc/RobotContainer.java | 55 ++++++------------- .../frc/subsystems/indexer/Disruptor.java | 2 +- .../frc/subsystems/indexer/Indexer.java | 2 +- src/main/java/org/team2342/lib | 2 +- 5 files changed, 23 insertions(+), 44 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 190caa3..d587716 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -155,10 +155,10 @@ 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(false) + .withMotorInverted(true) .withSupplyCurrentLimit(30.0) .withStatorCurrentLimit(60.0) .withIdleMode(MotorConfig.IdleMode.BRAKE); @@ -303,6 +303,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 a997395..07788c4 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -150,9 +150,9 @@ public RobotContainer() { disruptor = new Disruptor( new DumbMotorIOSparkFlex( - 0, + CANConstants.DISRUPTOR_ID, IndexerConstants.DISRUPTOR_MOTOR_CONFIG, - MotorType.kBrushless)); // TODO: can id + MotorType.kBrushless)); pivot = new Pivot( new SmartMotorIOTalonFX( @@ -415,8 +415,8 @@ private void configureBindings() { .rightTrigger() .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) .and(activeOrPassing) - .whileTrue(pulsedAntiJamFeed()) - .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())); // Firing during inactive period driverController @@ -428,8 +428,8 @@ private void configureBindings() { driverController .rightBumper() .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) - .whileTrue(pulsedAntiJamFeed()) - .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 @@ -471,8 +471,6 @@ private void configureBindings() { shiftAboutToEnd .and(RobotModeTriggers.teleop()) .onTrue(driverController.rumble(RumbleType.kRightRumble, 1.0).withTimeout(0.25)); - - } public Command getAutonomousCommand() { @@ -527,40 +525,19 @@ public static boolean withinBounds(double value, double bound1, double 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(); + 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(); + 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 index 067a303..513cebf 100644 --- a/src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java +++ b/src/main/java/org/team2342/frc/subsystems/indexer/Disruptor.java @@ -52,6 +52,6 @@ public Command stop() { } public boolean isJammed() { - return inputs.currentAmps > IndexerConstants.MAX_CURRENT; + 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 23fed19..85d67f8 100644 --- a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java @@ -59,6 +59,6 @@ public Command stop() { } public boolean isJammed() { - return inputs.currentAmps > IndexerConstants.MAX_CURRENT; + 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