diff --git a/src/main/java/frc/robot/commands/RunIntake.java b/src/main/java/frc/robot/commands/RunIntake.java index 6dff66b8..93479461 100644 --- a/src/main/java/frc/robot/commands/RunIntake.java +++ b/src/main/java/frc/robot/commands/RunIntake.java @@ -40,6 +40,9 @@ public void execute() {} @Override public void end(boolean interrupted) { intake.stop(); + if (!interrupted) { + intake.reportNoteIsInIntake(); + } } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/RunIntakeReverse.java b/src/main/java/frc/robot/commands/RunIntakeReverse.java index 24855d53..acc6f186 100644 --- a/src/main/java/frc/robot/commands/RunIntakeReverse.java +++ b/src/main/java/frc/robot/commands/RunIntakeReverse.java @@ -40,6 +40,7 @@ public void execute() {} @Override public void end(boolean interrupted) { intake.stop(); + intake.reportNoteIsNotInIntake(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/mailbox/DeindexNote.java b/src/main/java/frc/robot/commands/mailbox/DeindexNote.java index ad2fda71..7714d001 100644 --- a/src/main/java/frc/robot/commands/mailbox/DeindexNote.java +++ b/src/main/java/frc/robot/commands/mailbox/DeindexNote.java @@ -52,6 +52,7 @@ public void execute() { @Override public void end(boolean interrupted) { intake.stop(); + intake.reportNoteIsNotInIntake(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index e3e193e7..4d6433a7 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -14,7 +14,9 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -24,6 +26,7 @@ public class Intake extends SubsystemBase { private CANSparkMax intakeLower; private CANSparkMax intakeUpper; private DigitalInput limitSwitch; + private GenericEntry noteIsInIntake; /** Creates a new Intake. */ public Intake() { @@ -47,6 +50,8 @@ public Intake() { intakeLower.burnFlash(); intakeUpper.burnFlash(); + + Shuffleboard.getTab("Driver").add("Note in intake", false).getEntry(); } /** Runs the intake motors. */ @@ -69,6 +74,16 @@ public boolean getLimitSwitch() { return !limitSwitch.get(); } + /** Tells drivers the intake is full */ + public void reportNoteIsInIntake() { + noteIsInIntake.setBoolean(true); + } + + /** Tells drivers the intake is empty */ + public void reportNoteIsNotInIntake() { + noteIsInIntake.setBoolean(false); + } + /** Stops the intake motors. */ public void stop() { intakeUpper.set(0);