Ls stalled intake

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);
 
         }
 

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();
         }

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()) {

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;

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,27 +23,41 @@
 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;
     }
 
     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())<Constants.INTAKE_STALL_SPEED && stallTimer.hasElapsed(Constants.INTAKE_STALL_TIME))  {
+            intakeStalled = true;
+        } else {
+            intakeStalled = false;
+        }
     }
 
     public static SparkMaxIo createMockIo() {
@@ -78,4 +93,11 @@ private static SparkMax createMotor() {
                 PersistMode.kPersistParameters);
         return motor;
     }
+    public boolean getIntakeStalled() {
+        return intakeStalled;
+    }
+    public void startIntaking() {
+        if (!intaking)
+            stallTimer.reset();
+    }
 }

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));
+        setDefaultCommand(new SetLed(this, drivebase, shootingState, stalledIntake));
     }
 
     public void setPattern(BlinkinPattern pattern) {

src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java

@@ -42,6 +42,10 @@ public boolean isRevSwitchPressed() {
     public double getEncoderPosition() {
         return getInputs().getEncoderPosition();
     }
+    @Override
+    public double getVelocity() {
+        return getInputs().getEncoderVelocity();
+    }
 
     @Override
     protected void updateInputs(MotorLoggableInputs inputs) {

src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java

@@ -51,6 +51,10 @@ public boolean isRevSwitchPressed() {
     public double getEncoderPosition() {
         return getInputs().getEncoderPosition();
     }
+    @Override
+    public double getVelocity() {
+        return getInputs().getEncoderVelocity();
+    }
 
     @Override
     protected void updateInputs(MotorLoggableInputs inputs) {

src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java

@@ -15,4 +15,5 @@ public interface SparkMaxIo extends BaseIo {
     boolean isFwdSwitchPressed();
     boolean isRevSwitchPressed();
     double getEncoderPosition();
+    double getVelocity();
 }