Rumble to Intake

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

@@ -8,7 +8,6 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -19,6 +18,7 @@
 import frc.team2412.robot.commands.intake.AllStopCommand;
 import frc.team2412.robot.commands.intake.FeederInCommand;
 import frc.team2412.robot.commands.intake.IntakeRejectCommand;
+import frc.team2412.robot.commands.intake.RumbleCommand;
 import frc.team2412.robot.commands.launcher.FullTargetCommand;
 import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand;
 import frc.team2412.robot.commands.launcher.SetAngleCommand;
@@ -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 RumbleCommand(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));
@@ -176,8 +177,10 @@ private void bindLauncherControls() {
 	}
 
 	public void vibrateDriveController(double vibration) {
-		if (!DriverStation.isAutonomous()) {
-			driveController.getHID().setRumble(RumbleType.kBothRumble, 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;
@@ -47,8 +48,13 @@ 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
+	// control intake speed
 	private final GenericEntry setIntakeInSpeedEntry =
 			Shuffleboard.getTab("Intake")
 					.addPersistent("Intake in speed - ", INTAKE_IN_SPEED)
@@ -68,7 +74,7 @@ public class IntakeSubsystem extends SubsystemBase {
 					.withSize(1, 1)
 					.getEntry();
 
-	// temperature
+	// motor temperature
 	private final GenericEntry intakeMotorFrontTemp =
 			Shuffleboard.getTab("Intake")
 					.add("Front Intake temp", 0)
@@ -126,7 +132,16 @@ public class IntakeSubsystem extends SubsystemBase {
 					.withWidget(BuiltInWidgets.kToggleSwitch)
 					.getEntry();
 
+	// reject override
+	private final GenericEntry rejectOverride =
+			Shuffleboard.getTab("Intake")
+					.add("Override Intake Reject", false)
+					.withSize(1, 1)
+					.withWidget(BuiltInWidgets.kToggleSwitch)
+					.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,11 +155,26 @@ 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);
+
+		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);
+		// 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) {
@@ -189,14 +219,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));
@@ -237,10 +301,27 @@ 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);
 	}
 
+	public boolean getRejectOverride() {
+		return rejectOverride.getBoolean(false);
+	}
+
 	@Override
 	public void periodic() {
 		intakeMotorFrontTemp.setDouble(intakeMotorFront.getMotorTemperature());