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/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/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index 4f39c8d9..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,9 @@ 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; public class SetLed extends LoggableCommand{ @@ -15,12 +18,14 @@ public class SetLed extends LoggableCommand{ private ShootingState shootingState; private double x; private double y; + private BooleanSupplier stalledIntake; - public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState) { + public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState, BooleanSupplier stalledIntake) { this.lightStrip = lightStrip; this.drivebase = drivebase; this.shootingState = shootingState; + this.stalledIntake = stalledIntake; addRequirements(lightStrip); } @@ -32,7 +37,7 @@ public void initialize() { @Override public void execute() { - + Logger.recordOutput("IntakeJammed", stalledIntake); x = drivebase.getPose().getX(); y = drivebase.getPose().getY(); @@ -46,6 +51,8 @@ public void execute() { lightStrip.setPattern(BlinkinPattern.STROBE_RED); } + } else if (stalledIntake.getAsBoolean()) { + lightStrip.setPattern(BlinkinPattern.ORANGE); } else { switch (shootingState.getShootState()) { diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index b69bdc9b..b85b6b0b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -46,6 +46,8 @@ 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 13aa5c8c..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; @@ -22,11 +23,17 @@ 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; + private Timer stallTimer = new Timer(); public IntakeSubsystem(SparkMaxIo io) { this.io = io; @@ -34,15 +41,23 @@ 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())