Rumble to Intake

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

@@ -131,11 +131,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));
@@ -211,6 +212,7 @@ private void bindSysIdControls() {
 	public void vibrateDriveController(double vibration) {
 		if (!DriverStation.isAutonomous()) {
 			driveController.getHID().setRumble(RumbleType.kBothRumble, vibration);
+			codriveController.getHID().setRumble(RumbleType.kBothRumble, vibration);
 		}
 	}
 }

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

@@ -1,13 +1,22 @@
 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;
+	private boolean rumbledIntakeFront = false;
+	private boolean rumbledIntakeLeft = false;
+	private boolean rumbledIntakeRight = false;
+	private boolean rumbledIndex = false;
 
-	public AllInCommand(IntakeSubsystem intakeSubsystem) {
+	public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) {
 		this.intakeSubsystem = intakeSubsystem;
+		this.controls = controls;
 		addRequirements(intakeSubsystem);
 	}
 
@@ -20,16 +29,94 @@ public void initialize() {
 
 	@Override
 	public void execute() {
+		// intake rejecting
+		if (intakeSubsystem.intakeFrontSeesNote()) {
+			if (!intakeSubsystem.getRejectOverride()) {
+				intakeSubsystem.intakeBackReject();
+				intakeSubsystem.intakeLeftReject();
+				intakeSubsystem.intakeRightReject();
+			} else {
+				intakeSubsystem.intakeBackStop();
+				intakeSubsystem.intakeLeftStop();
+				intakeSubsystem.intakeRightStop();
+			}
+
+			if (controls != null && !rumbledIntakeFront) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
+				rumbledIntakeFront = true;
+			}
+		}
+
+		// back intake sensor does not exist (but it might?)
+
+		if (intakeSubsystem.intakeLeftSeesNote()) {
+			if (!intakeSubsystem.getRejectOverride()) {
+				intakeSubsystem.intakeFrontReject();
+				intakeSubsystem.intakeBackReject();
+				intakeSubsystem.intakeRightReject();
+			} else {
+				intakeSubsystem.intakeFrontStop();
+				intakeSubsystem.intakeBackStop();
+				intakeSubsystem.intakeRightStop();
+			}
+
+			if (controls != null && !rumbledIntakeLeft) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
+				rumbledIntakeLeft = true;
+			}
+		}
+
+		if (intakeSubsystem.intakeRightSeesNote()) {
+			if (!intakeSubsystem.getRejectOverride()) {
+				intakeSubsystem.intakeFrontReject();
+				intakeSubsystem.intakeBackReject();
+				intakeSubsystem.intakeLeftReject();
+			} else {
+				intakeSubsystem.intakeFrontStop();
+				intakeSubsystem.intakeBackStop();
+				intakeSubsystem.intakeLeftStop();
+			}
+
+			if (controls != null && !rumbledIntakeRight) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
+				rumbledIntakeRight = true;
+			}
+		}
+
+		// all intake motors rejecting after index
 		if (intakeSubsystem.indexSensorHasNote()) {
-			intakeSubsystem.intakeReject();
+			if (!intakeSubsystem.getRejectOverride()) {
+				intakeSubsystem.intakeReject();
+			} else {
+				intakeSubsystem.intakeStop();
+			}
+
+			if (controls != null && !rumbledIndex) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule();
+				rumbledIndex = true;
+			}
 		}
 	}
 
 	@Override
 	public void end(boolean interrupted) {
-		// intakeSubsystem.intakeReject();
+		if (!intakeSubsystem.getRejectOverride()) {
+			intakeSubsystem.intakeReject();
+		} else {
+			intakeSubsystem.intakeStop();
+		}
+
 		intakeSubsystem.indexStop();
 		intakeSubsystem.feederStop();
+
+		if (controls != null) {
+			Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule();
+		}
+
+		rumbledIntakeFront = false;
+		rumbledIntakeLeft = false;
+		rumbledIntakeRight = false;
+		rumbledIndex = false;
 	}
 
 	@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/commands/launcher/FullTargetCommand.java

@@ -1,5 +1,7 @@
 package frc.team2412.robot.commands.launcher;
 
+import static frc.team2412.robot.Subsystems.SubsystemConstants.*;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -38,7 +40,9 @@ public FullTargetCommand(
 		this.launcherSubsystem = launcherSubsystem;
 		this.drivebaseSubsystem = drivebaseSubsystem;
 		this.controls = controls;
-		yawAlignmentCommand = drivebaseSubsystem.rotateToAngle(() -> yawTarget, false);
+		if (DRIVEBASE_ENABLED) {
+			yawAlignmentCommand = drivebaseSubsystem.rotateToAngle(() -> yawTarget, false);
+		}
 
 		addRequirements(launcherSubsystem);
 	}

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;
@@ -48,6 +49,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
 
 	private final ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake");
@@ -61,7 +67,11 @@ public class IntakeSubsystem extends SubsystemBase {
 	// sensor override
 	private GenericEntry sensorOverride;
 
+	// reject override
+	private GenericEntry rejectOverride;
+
 	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);
@@ -75,6 +85,17 @@ 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);
+
+		// todo: MOVE THIS TO CONFIGURE MOTOR
+		intakeFrontSensor.enableLimitSwitch(false);
+		intakeLeftSensor.enableLimitSwitch(false);
+		intakeRightSensor.enableLimitSwitch(false);
+
 		resetMotors();
 
 		initShuffleboard();
@@ -126,14 +147,48 @@ public void intakeReverse() {
 		intakeSet(INTAKE_REVERSE_SPEED);
 	}
 
+	// intake stop methods
 	public void intakeStop() {
 		intakeSet(0);
 	}
 
+	public void intakeFrontStop() {
+		intakeMotorFront.set(0);
+	}
+
+	public void intakeBackStop() {
+		intakeMotorBack.set(0);
+	}
+
+	public void intakeLeftStop() {
+		intakeMotorLeft.set(0);
+	}
+
+	public void intakeRightStop() {
+		intakeMotorRight.set(0);
+	}
+
+	// intake reject methods
 	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));
@@ -174,12 +229,28 @@ public boolean feederSensorHasNote() {
 		return !feederSensor.get() && !getSensorOverride();
 	}
 
+	public boolean intakeFrontSeesNote() {
+		return intakeFrontSensor.isPressed();
+	}
+
+	public boolean intakeLeftSeesNote() {
+		return intakeLeftSensor.isPressed();
+	}
+
+	public boolean intakeRightSeesNote() {
+		return intakeRightSensor.isPressed();
+	}
+
+	// override methods on shuffleboard
 	public boolean getSensorOverride() {
 		return sensorOverride.getBoolean(false);
 	}
 
-	// logging
+	public boolean getRejectOverride() {
+		return rejectOverride.getBoolean(false);
+	}
 
+	// logging
 	public void initShuffleboard() {
 		if (Robot.isDebugMode()) {
 			shuffleboardTab
@@ -215,6 +286,11 @@ public void initShuffleboard() {
 		shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1);
 		shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1);
 
+		// no 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);
+
 		setIntakeInSpeedEntry =
 				shuffleboardTab
 						.addPersistent("Intake in speed - ", INTAKE_IN_SPEED)
@@ -234,5 +310,12 @@ public void initShuffleboard() {
 						.withSize(1, 1)
 						.withWidget(BuiltInWidgets.kToggleSwitch)
 						.getEntry();
+
+		rejectOverride =
+				Shuffleboard.getTab("Intake")
+						.add("Override Intake Reject", false)
+						.withSize(1, 1)
+						.withWidget(BuiltInWidgets.kToggleSwitch)
+						.getEntry();
 	}
 }

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(