diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index c5b39973..c71fbdd1 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -14,8 +14,8 @@ public static class SubsystemConstants { private static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean APRILTAGS_ENABLED = true; - public static final boolean LIMELIGHT_ENABLED = true; - public static final boolean CLIMB_ENABLED = true; + public static final boolean LIMELIGHT_ENABLED = false; + public static final boolean CLIMB_ENABLED = false; public static final boolean LAUNCHER_ENABLED = true; public static final boolean INTAKE_ENABLED = true; public static final boolean DRIVEBASE_ENABLED = true; diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 57a0f6ae..f5b950f7 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -9,6 +9,10 @@ public class AllInCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final Controls controls; + private boolean rumbledIntakeFront = false; + private boolean rumbledIntakeLeft = false; + private boolean rumbledIntakeRight = false; + private boolean rumbledIndex = false; public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) { this.intakeSubsystem = intakeSubsystem; @@ -37,27 +41,14 @@ public void execute() { intakeSubsystem.intakeRightStop(); } - if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); - } - } - - if (intakeSubsystem.intakeBackSeesNote()) { - if (!intakeSubsystem.getRejectOverride()) { - intakeSubsystem.intakeFrontReject(); - intakeSubsystem.intakeLeftReject(); - intakeSubsystem.intakeRightReject(); - } else { - intakeSubsystem.intakeFrontStop(); - intakeSubsystem.intakeLeftStop(); - intakeSubsystem.intakeRightStop(); - } - - if (controls != null) { + if (controls != null && !rumbledIntakeFront) { Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeFront = true; } } + // back intake sensor does not exist ._. + if (intakeSubsystem.intakeLeftSeesNote()) { if (!intakeSubsystem.getRejectOverride()) { intakeSubsystem.intakeFrontReject(); @@ -69,8 +60,9 @@ public void execute() { intakeSubsystem.intakeRightStop(); } - if (controls != null) { + if (controls != null && !rumbledIntakeLeft) { Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeLeft = true; } } @@ -85,8 +77,9 @@ public void execute() { intakeSubsystem.intakeLeftStop(); } - if (controls != null) { + if (controls != null && !rumbledIntakeRight) { Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeRight = true; } } @@ -98,8 +91,9 @@ public void execute() { intakeSubsystem.intakeStop(); } - if (controls != null) { + if (controls != null && !rumbledIndex) { Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); + rumbledIndex = true; } } } @@ -111,6 +105,7 @@ public void end(boolean interrupted) { } else { intakeSubsystem.intakeStop(); } + intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index ec4f6077..a1717db7 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -49,7 +49,7 @@ public class IntakeSubsystem extends SubsystemBase { private final DigitalInput feederSensor; private final SparkLimitSwitch intakeFrontSensor; - private final SparkLimitSwitch intakeBackSensor; + // private final SparkLimitSwitch intakeBackSensor; private final SparkLimitSwitch intakeLeftSensor; private final SparkLimitSwitch intakeRightSensor; @@ -156,16 +156,25 @@ public IntakeSubsystem() { feederSensor = new DigitalInput(FEEDER_SENSOR); intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - intakeBackSensor = intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakeBackSensor = + // intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeFrontSensor.enableLimitSwitch(false); + intakeLeftSensor.enableLimitSwitch(false); + intakeRightSensor.enableLimitSwitch(false); + resetMotors(); + // show if sensors have notes in shuffleboard ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake"); shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1); shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1); - shuffleboardTab.addBoolean("Intake Sensor - ", this::intakeBackSeesNote).withSize(1, 1); + // intake back sensor + shuffleboardTab.addBoolean("Intake Front Sensor - ", this::intakeFrontSeesNote).withSize(1, 1); + shuffleboardTab.addBoolean("Intake Left Sensor - ", this::intakeLeftSeesNote).withSize(1, 1); + shuffleboardTab.addBoolean("Intake Right Sensor - ", this::intakeRightSeesNote).withSize(1, 1); } private void configureMotor(CANSparkBase motor, int currentLimit, boolean invert) { @@ -296,10 +305,6 @@ public boolean intakeFrontSeesNote() { return intakeFrontSensor.isPressed(); } - public boolean intakeBackSeesNote() { - return intakeBackSensor.isPressed(); - } - public boolean intakeLeftSeesNote() { return intakeLeftSensor.isPressed(); }