Rumble to Intake

src/main/java/frc/team2412/robot/Controls.java

@@ -122,11 +122,12 @@ private void bindDrivebaseControls() {
 	private void bindIntakeControls() {
 		// CommandScheduler.getInstance()
 		// 		.setDefaultCommand(s.intakeSubsystem, new IntakeStopCommand(s.intakeSubsystem));
-		driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem));
-		// driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem));
+		driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this));
+		driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem));
+
 		driveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem));
 		driveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem));
-		codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem));
+		codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this));
 		codriveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem));
 		codriveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem));
 		codriveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem));

src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java

@@ -1,13 +1,18 @@
 package frc.team2412.robot.commands.intake;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.team2412.robot.Controls;
 import frc.team2412.robot.subsystems.IntakeSubsystem;
 
 public class AllInCommand extends Command {
 	private final IntakeSubsystem intakeSubsystem;
+	private final Controls controls;
 
-	public AllInCommand(IntakeSubsystem intakeSubsystem) {
+	public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) {
 		this.intakeSubsystem = intakeSubsystem;
+		this.controls = controls;
 		addRequirements(intakeSubsystem);
 	}
 
@@ -20,16 +25,66 @@ public void initialize() {
 
 	@Override
 	public void execute() {
+		// intake rejecting
+		if (intakeSubsystem.intakeFrontSeesNote()) {
+			intakeSubsystem.intakeBackReject();
+			intakeSubsystem.intakeLeftReject();
+			intakeSubsystem.intakeRightReject();
+
+			if (controls != null) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule();
+			}
+		}
+
+		if (intakeSubsystem.intakeBackSeesNote()) {
+			intakeSubsystem.intakeFrontReject();
+			intakeSubsystem.intakeLeftReject();
+			intakeSubsystem.intakeRightReject();
+
+			if (controls != null) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule();
+			}
+		}
+
+		if (intakeSubsystem.intakeLeftSeesNote()) {
+			intakeSubsystem.intakeFrontReject();
+			intakeSubsystem.intakeBackReject();
+			intakeSubsystem.intakeRightReject();
+
+			if (controls != null) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule();
+			}
+		}
+
+		if (intakeSubsystem.intakeRightSeesNote()) {
+			intakeSubsystem.intakeFrontReject();
+			intakeSubsystem.intakeBackReject();
+			intakeSubsystem.intakeLeftReject();
+
+			if (controls != null) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule();
+			}
+		}
+
+		// all intake motors rejecting after index
 		if (intakeSubsystem.indexSensorHasNote()) {
 			intakeSubsystem.intakeReject();
+
+			if (controls != null) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule();
+			}
 		}
 	}
 
 	@Override
 	public void end(boolean interrupted) {
-		// intakeSubsystem.intakeReject();
+		intakeSubsystem.intakeReject();
 		intakeSubsystem.indexStop();
 		intakeSubsystem.feederStop();
+
+		if (controls != null) {
+			Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule();
+		}
 	}
 
 	@Override

src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java

@@ -0,0 +1,25 @@
+package frc.team2412.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.IntakeSubsystem;
+
+public class AllInNoSensorCommand extends Command {
+	private final IntakeSubsystem intakeSubsystem;
+
+	public AllInNoSensorCommand(IntakeSubsystem intakeSubsystem) {
+		this.intakeSubsystem = intakeSubsystem;
+		addRequirements(intakeSubsystem);
+	}
+
+	@Override
+	public void initialize() {
+		intakeSubsystem.intakeIn();
+		intakeSubsystem.indexIn();
+		intakeSubsystem.feederIn();
+	}
+
+	@Override
+	public boolean isFinished() {
+		return true;
+	}
+}

src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java

@@ -0,0 +1,27 @@
+package frc.team2412.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.Controls;
+
+public class RumbleCommand extends Command {
+	private final Controls controls;
+
+	public RumbleCommand(Controls controls) {
+		this.controls = controls;
+	}
+
+	@Override
+	public void initialize() {
+		controls.vibrateDriveController(1.0);
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		controls.vibrateDriveController(0.0);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java

@@ -7,6 +7,7 @@
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkLimitSwitch;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
@@ -47,6 +48,11 @@ public class IntakeSubsystem extends SubsystemBase {
 	private final DigitalInput indexSensor;
 	private final DigitalInput feederSensor;
 
+	private final SparkLimitSwitch intakeFrontSensor;
+	private final SparkLimitSwitch intakeBackSensor;
+	private final SparkLimitSwitch intakeLeftSensor;
+	private final SparkLimitSwitch intakeRightSensor;
+
 	// Shuffleboard
 	// speed
 	private final GenericEntry setIntakeInSpeedEntry =
@@ -127,6 +133,7 @@ public class IntakeSubsystem extends SubsystemBase {
 					.getEntry();
 
 	public IntakeSubsystem() {
+
 		intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless);
 		intakeMotorBack = new CANSparkMax(INTAKE_MOTOR_BACK, MotorType.kBrushless);
 		intakeMotorLeft = new CANSparkMax(INTAKE_MOTOR_LEFT, MotorType.kBrushless);
@@ -140,6 +147,11 @@ public IntakeSubsystem() {
 		indexSensor = new DigitalInput(INDEX_SENSOR);
 		feederSensor = new DigitalInput(FEEDER_SENSOR);
 
+		intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
+		intakeBackSensor = intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
+		intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
+		intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
+
 		resetMotors();
 
 		ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake");
@@ -197,6 +209,22 @@ public void intakeReject() {
 		intakeSet(INTAKE_REJECT_SPEED);
 	}
 
+	public void intakeFrontReject() {
+		intakeMotorFront.set(INTAKE_REJECT_SPEED);
+	}
+
+	public void intakeBackReject() {
+		intakeMotorBack.set(INTAKE_REJECT_SPEED);
+	}
+
+	public void intakeLeftReject() {
+		intakeMotorLeft.set(INTAKE_REJECT_SPEED);
+	}
+
+	public void intakeRightReject() {
+		intakeMotorRight.set(INTAKE_REJECT_SPEED);
+	}
+
 	// index methods
 	public void indexIn() {
 		indexMotorUpper.set(setIndexInSpeedEntry.getDouble(INDEX_UPPER_IN_SPEED));
@@ -237,6 +265,22 @@ public boolean feederSensorHasNote() {
 		return !feederSensor.get() && !getSensorOverride();
 	}
 
+	public boolean intakeFrontSeesNote() {
+		return intakeFrontSensor.isPressed();
+	}
+
+	public boolean intakeBackSeesNote() {
+		return intakeBackSensor.isPressed();
+	}
+
+	public boolean intakeLeftSeesNote() {
+		return intakeLeftSensor.isPressed();
+	}
+
+	public boolean intakeRightSeesNote() {
+		return intakeRightSensor.isPressed();
+	}
+
 	public boolean getSensorOverride() {
 		return sensorOverride.getBoolean(false);
 	}

src/main/java/frc/team2412/robot/util/AutoLogic.java

@@ -52,7 +52,7 @@ public void registerCommands() {
 
 		// Intake
 		NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem));
-		NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem));
+		NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, null));
 		NamedCommands.registerCommand(
 				"IntakeSensorOverride", new AllInSensorOverrideCommand(s.intakeSubsystem));
 		// Launcher
@@ -74,7 +74,6 @@ public void registerCommands() {
 		NamedCommands.registerCommand(
 				"RetractPivot",
 				new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0)); // TODO: add retract angle
-
 		// Complex Autos
 		NamedCommands.registerCommand("AutoLogicTest", AutoPaths.testAuto);
 		NamedCommands.registerCommand(