Added rumble

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -82,7 +82,6 @@
 public class RobotContainer implements Sendable {
 
     public final GenericHID driverController = new GenericHID(Driver.port);
-    public final XboxController driverRumble = new XboxController(Driver.port); //For rumbling the controller
     public final GenericHID manipulatorController = new GenericHID(Manipulator.port);
 
     public final Limelight limelight = new Limelight();
@@ -146,7 +145,7 @@ private void setBindingsManipulator() {
       .whileTrue(new RunConveyor(intake)); //could be toggle mode instead
    // .whileTrue(new OuttakeFeeder(outtake)); //could be toggle mode instead
      new JoystickButton(manipulatorController, Manipulator.INTAKE_BUTTON)
-      .whileTrue(new IntakeBalls(intake));
+      .whileTrue(new IntakeBalls(intake, manipulatorController));
       new JoystickButton(manipulatorController, Manipulator.OUTTAKE_BUTTON)
       .whileTrue(new ShootBalls(outtake));
       axisTrigger(manipulatorController, Manipulator.SMART_SHOOT_AXIS, OI.JOY_THRESH)
@@ -172,7 +171,7 @@ private void setDefaultCommands() {
       () -> SmartDashboard.getBoolean("Baby Mode", Config.CONFIG.isBabyMode())
       ));
 
-      intake.setDefaultCommand(new IntakeBalls(intake));
+      intake.setDefaultCommand(new IntakeBalls(intake, manipulatorController));
     }
   //#endregion
   //#region getAutoCommand

src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java

@@ -8,15 +8,21 @@
 
 import org.carlmontrobotics.subsystems.Intake;
 
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class IntakeBalls extends Command {
   Intake intake;
+  GenericHID manipulatorRumble;
 
   /** Creates a new IntakeBalls. */
-  public IntakeBalls(Intake intake) {
+  public IntakeBalls(Intake intake, GenericHID manipulatorRumble) {
     this.intake = intake;
+    this.manipulatorRumble = manipulatorRumble;
     addRequirements(intake);
     // Use addRequirements() here to declare subsystem dependencies.
   }
@@ -29,17 +35,31 @@ public void initialize() {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if (checkIfJammed()) {
+      SmartDashboard.putBoolean("IntakeJammed", true);
+      manipulatorRumble.setRumble(RumbleType.kRightRumble, 0.5);
+    }
+    else {
+      SmartDashboard.putBoolean("IntakeJammed", true);
+      manipulatorRumble.setRumble(RumbleType.kRightRumble, 0);
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     intake.stopIntake();
+    manipulatorRumble.setRumble(RumbleType.kRightRumble, 0);
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
     return false;
   }
+
+  private boolean checkIfJammed() {
+    return intake.getIntakeVelocity() < 20;
+  }
 }

src/main/java/org/carlmontrobotics/subsystems/Intake.java

@@ -62,4 +62,28 @@ public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("rpm brr", intakeMotor.getEncoder().getVelocity());
   }
+
+  /**
+   * 
+   * @return Number the RPM of the motor
+   */
+  public double getIntakeVelocity() {
+    return intakeMotor.getEncoder().getVelocity();
+  }
+
+  /**
+   * 
+   * @return The motor controller's output current in Amps.
+   */
+  public double getIntakeCurrent() {
+    return intakeMotor.getOutputCurrent();
+  }
+  /**
+   * 
+   * @return The motor controller's applied output duty cycle.
+
+   */
+  public double getIntakeAppliedOutput() {
+    return intakeMotor.getAppliedOutput();
+  }
 }