From 856c0a262a7a1d74a11c327020422d53576a1939 Mon Sep 17 00:00:00 2001 From: moo Date: Sun, 5 Apr 2026 16:25:04 -0700 Subject: [PATCH] stuffing, reverses intake when unjamming --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/commands/gpm/ReverseMotors.java | 5 +---- .../frc/robot/commands/gpm/RunSpindexer.java | 18 +++++++++++++++++- .../controls/PS5ControllerDriverConfig.java | 4 ++-- .../frc/robot/subsystems/Intake/Intake.java | 4 ++++ .../robot/subsystems/spindexer/Spindexer.java | 2 +- 6 files changed, 27 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index db87084..5dcbaee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -246,8 +246,8 @@ public class RobotContainer { })); } - if (turret != null && drive != null && hood != null && shooter != null && spindexer != null) { - Command runSpindexer = new RunSpindexer(spindexer, turret, hood); + if (turret != null && drive != null && hood != null && shooter != null && spindexer != null && intake != null) { + Command runSpindexer = new RunSpindexer(spindexer, turret, hood, intake); NamedCommands.registerCommand("Auto shoot", new AutoShootCommand(turret, drive, hood, shooter, spindexer)); NamedCommands.registerCommand("Start Spindexer", new InstantCommand(() -> CommandScheduler.getInstance().schedule(runSpindexer))); diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java index 05ae1b6..2ed506d 100644 --- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java +++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java @@ -2,16 +2,13 @@ package frc.robot.commands.gpm; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Intake.Intake; -import frc.robot.subsystems.spindexer.Spindexer; public class ReverseMotors extends Command { private Intake intake; - private Spindexer spindexer; - public ReverseMotors(Intake intake, Spindexer spindexer){ + public ReverseMotors(Intake intake){ this.intake = intake; - this.spindexer = spindexer; addRequirements(intake); } diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java index 4a6ac7c..adc423f 100644 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java @@ -5,7 +5,10 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; import frc.robot.constants.Constants; +import frc.robot.constants.IntakeConstants; +import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.spindexer.Spindexer; import frc.robot.subsystems.spindexer.SpindexerConstants; @@ -15,6 +18,7 @@ public class RunSpindexer extends Command { private Spindexer spindexer; private Turret turret; private Hood hood; + private Intake intake; private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising); // if there is jam I would think this is 0 -> 1 @@ -22,11 +26,14 @@ public class RunSpindexer extends Command { private boolean wasHoodForcedDown = false; private Timer reverseTimer = new Timer(); + + private double storedIntakeSpeed = 0.0; - public RunSpindexer(Spindexer spindexer, Turret turret, Hood hood) { + public RunSpindexer(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { this.spindexer = spindexer; this.turret = turret; this.hood = hood; + this.intake = intake; addRequirements(spindexer); } @@ -60,13 +67,22 @@ public class RunSpindexer extends Command { reversing = true; reverseTimer.reset(); reverseTimer.start(); + storedIntakeSpeed = intake.getSpeed(); } if (!reversing) { spindexer.maxSpindexer(); } else { spindexer.reverseSpindexer(); + + if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) { + intake.spinReverse(); + } else { + intake.extend(); + } + if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) { reversing = false; + intake.spin(storedIntakeSpeed); } } if (!Constants.DISABLE_SMART_DASHBOARD) { diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 07fba40..ec1916f 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -124,11 +124,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { } // Spindexer - if (spindexer != null && turret != null && hood != null) { + if (spindexer != null && turret != null && hood != null && intake != null) { // Toggle spindexer controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue( - new RunSpindexer(spindexer, turret, hood) + new RunSpindexer(spindexer, turret, hood, intake) ); } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index bfbd44d..b0eea66 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -302,6 +302,10 @@ public class Intake extends SubsystemBase implements IntakeIO{ rollerMotor.set(speed); } + public double getSpeed() { + return rollerMotor.get(); + } + /** * Start the intake roller spinning. */ diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 6b49f3b..4cba8ed 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -94,7 +94,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { wasSpindexerSlow = isSpindexerSlow; if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Spindexer Jamming", state == SpindexerState.REVERSE); + SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE); } } -- 2.39.5