From 3bbb647d0d2de1d4b75d01dedd0242c7c4a8376b Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 21 Mar 2026 17:03:38 -0400 Subject: [PATCH 1/7] should be fixed need to test --- src/main/java/frc/robot/constants/GameConstants.java | 1 + .../java/frc/robot/subsystems/IntakeSubsystem.java | 11 +++++++++++ .../robot/utils/logging/io/motor/MockSparkMaxIo.java | 4 ++++ .../robot/utils/logging/io/motor/RealSparkMaxIo.java | 4 ++++ .../frc/robot/utils/logging/io/motor/SparkMaxIo.java | 1 + 5 files changed, 21 insertions(+) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 89565c75..79ba7ac1 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -46,6 +46,7 @@ public enum Mode { //Speeds public static final double INTAKE_SPEED = -0.5; + public static final double INTAKE_STALL_SPEED = 0.1; public static final double HOPPER_SPEED = 0.35;//Want to increase this later public static final double CLIMBER_SPEED_UP = 0.1; public static final double CLIMBER_SPEED_DOWN = -0.1; diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 13aa5c8c..26a979b3 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -22,11 +22,16 @@ import frc.robot.utils.logging.io.motor.SparkMaxIo; import frc.robot.utils.simulation.MotorSimulator; import frc.robot.utils.simulation.RobotVisualizer; +import org.littletonrobotics.junction.Logger; + +import static java.lang.Math.abs; public class IntakeSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "IntakeSubsystem"; private final SparkMaxIo io; + private boolean intakeStalled = false; + private boolean intaking = false; public IntakeSubsystem(SparkMaxIo io) { this.io = io; @@ -34,15 +39,21 @@ public IntakeSubsystem(SparkMaxIo io) { public void setSpeed(double speed) { io.set(speed); + intaking = true; } public void stopMotors() { io.stopMotor(); + intaking = false; + intakeStalled = false; } @Override public void periodic() { io.periodic(); + if (intaking && abs(io.getVelocity()) Date: Sat, 21 Mar 2026 17:06:40 -0400 Subject: [PATCH 2/7] added led --- src/main/java/frc/robot/commands/lightStrip/SetLed.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index 4f39c8d9..54a9297a 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLed.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -15,12 +15,14 @@ public class SetLed extends LoggableCommand{ private ShootingState shootingState; private double x; private double y; + private boolean stalledIntake; - public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState) { + public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState, boolean stalledIntake) { this.lightStrip = lightStrip; this.drivebase = drivebase; this.shootingState = shootingState; + this.stalledIntake = stalledIntake; addRequirements(lightStrip); } @@ -46,6 +48,8 @@ public void execute() { lightStrip.setPattern(BlinkinPattern.STROBE_RED); } + } else if (stalledIntake) { + lightStrip.setPattern(BlinkinPattern.ORANGE); } else { switch (shootingState.getShootState()) { From dc13e53fa9010e3f874df6c3b6adf84b00d551c3 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 21 Mar 2026 17:15:30 -0400 Subject: [PATCH 3/7] wip --- src/main/java/frc/robot/subsystems/LightStripSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java index 880cce68..0e04ca80 100644 --- a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java @@ -16,7 +16,7 @@ public class LightStripSubsystem extends SubsystemBase{ public LightStripSubsystem(SwerveSubsystem drivebase, ShootingState shootingState) { this.io = new Spark(Constants.LIGHT_STRIP_CHANNEL); - setDefaultCommand(new SetLed(this, drivebase, shootingState)); + setDefaultCommand(new SetLed(this, drivebase, shootingState, false)); } public void setPattern(BlinkinPattern pattern) { From f6dcbeb1d4d26c42eac052f77134b3b41e035668 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 22 Mar 2026 13:36:34 -0400 Subject: [PATCH 4/7] should be ready for testing --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/commands/lightStrip/SetLed.java | 8 +++++--- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 3 +++ .../java/frc/robot/subsystems/LightStripSubsystem.java | 6 ++++-- 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69b284a7..35cc3ad8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -153,7 +153,7 @@ public RobotContainer() { new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; - lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState, intakeSubsystem::getIntakeStalled); } case REPLAY -> { anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createMockIo()); @@ -170,7 +170,7 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; - lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState, intakeSubsystem::getIntakeStalled); } case SIM -> { @@ -189,7 +189,7 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null; - lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState, intakeSubsystem::getIntakeStalled); } diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index 54a9297a..d9f85d06 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLed.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -8,6 +8,8 @@ import frc.robot.utils.BlinkinPattern; import frc.robot.utils.logging.commands.LoggableCommand; +import java.util.function.BooleanSupplier; + public class SetLed extends LoggableCommand{ private final LightStripSubsystem lightStrip; @@ -15,9 +17,9 @@ public class SetLed extends LoggableCommand{ private ShootingState shootingState; private double x; private double y; - private boolean stalledIntake; + private BooleanSupplier stalledIntake; - public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState, boolean stalledIntake) { + public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState, BooleanSupplier stalledIntake) { this.lightStrip = lightStrip; this.drivebase = drivebase; @@ -48,7 +50,7 @@ public void execute() { lightStrip.setPattern(BlinkinPattern.STROBE_RED); } - } else if (stalledIntake) { + } else if (stalledIntake.getAsBoolean()) { lightStrip.setPattern(BlinkinPattern.ORANGE); } else { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 26a979b3..01646794 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -89,4 +89,7 @@ private static SparkMax createMotor() { PersistMode.kPersistParameters); return motor; } + public boolean getIntakeStalled() { + return intakeStalled; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java index 0e04ca80..832cecee 100644 --- a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java @@ -8,15 +8,17 @@ import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.BlinkinPattern; +import java.util.function.BooleanSupplier; + public class LightStripSubsystem extends SubsystemBase{ public static final String LOGGING_NAME = "LightStripSubsystem"; private final Spark io; private BlinkinPattern pattern; - public LightStripSubsystem(SwerveSubsystem drivebase, ShootingState shootingState) { + public LightStripSubsystem(SwerveSubsystem drivebase, ShootingState shootingState, BooleanSupplier stalledIntake) { this.io = new Spark(Constants.LIGHT_STRIP_CHANNEL); - setDefaultCommand(new SetLed(this, drivebase, shootingState, false)); + setDefaultCommand(new SetLed(this, drivebase, shootingState, stalledIntake)); } public void setPattern(BlinkinPattern pattern) { From 71cd10433f20141611885cd0fb0c6a97e334e739 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 22 Mar 2026 15:22:35 -0400 Subject: [PATCH 5/7] small change --- src/main/java/frc/robot/commands/lightStrip/SetLed.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index d9f85d06..ba4de25d 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLed.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -7,6 +7,7 @@ import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.BlinkinPattern; import frc.robot.utils.logging.commands.LoggableCommand; +import org.littletonrobotics.junction.Logger; import java.util.function.BooleanSupplier; @@ -36,7 +37,7 @@ public void initialize() { @Override public void execute() { - + Logger.recordOutput("IntakeJammed", stalledIntake); x = drivebase.getPose().getX(); y = drivebase.getPose().getY(); From c541dcc9d5ca1590b31ad2449fbdc45457557865 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 22 Mar 2026 16:27:33 -0400 Subject: [PATCH 6/7] wip --- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 01646794..72d84639 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -53,6 +53,8 @@ public void periodic() { io.periodic(); if (intaking && abs(io.getVelocity()) Date: Mon, 23 Mar 2026 19:42:02 -0400 Subject: [PATCH 7/7] fixed --- src/main/java/frc/robot/commands/intake/SpinIntake.java | 1 + src/main/java/frc/robot/constants/GameConstants.java | 1 + src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 8 +++++++- 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/intake/SpinIntake.java b/src/main/java/frc/robot/commands/intake/SpinIntake.java index 5ceb0d86..35d19007 100644 --- a/src/main/java/frc/robot/commands/intake/SpinIntake.java +++ b/src/main/java/frc/robot/commands/intake/SpinIntake.java @@ -24,6 +24,7 @@ public void initialize() { public void execute() { if (intakeDeployer.getDeploymentState() == DeploymentState.DOWN) { subsystem.setSpeed(Constants.INTAKE_SPEED); + subsystem.startIntaking(); }else{ subsystem.stopMotors(); } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 18ba62e3..b85b6b0b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -47,6 +47,7 @@ public enum Mode { //Speeds public static final double INTAKE_SPEED = -0.5; public static final double INTAKE_STALL_SPEED = 0.1; + public static final double INTAKE_STALL_TIME = 0.1; public static final double HOPPER_SPEED = 0.35;//Want to increase this later public static final double CLIMBER_SPEED_UP = 0.1; public static final double CLIMBER_SPEED_DOWN = -0.1; diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 72d84639..5e3857c6 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -7,6 +7,7 @@ import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.commands.intake.SpinIntake; @@ -32,6 +33,7 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkMaxIo io; private boolean intakeStalled = false; private boolean intaking = false; + private Timer stallTimer = new Timer(); public IntakeSubsystem(SparkMaxIo io) { this.io = io; @@ -51,7 +53,7 @@ public void stopMotors() { @Override public void periodic() { io.periodic(); - if (intaking && abs(io.getVelocity())