Added rumble for intaking a note

POBots-353 · Apr 11, 2024 · 8b6a926 · 8b6a926
1 parent 83c378d
commit 8b6a926
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 0 deletions.
diff --git a/src/main/java/frc/lib/controllers/VirtualXboxController.java b/src/main/java/frc/lib/controllers/VirtualXboxController.java
@@ -1,10 +1,12 @@
 package frc.lib.controllers;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Axis;
 import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -43,6 +45,13 @@ public VirtualXboxController(int port) {
                 .ignoringDisable(true));
   }
 
+  public Command rumbleFor(double duration, RumbleType rumbleType, double value) {
+    return Commands.sequence(
+            Commands.runOnce(() -> getHID().setRumble(rumbleType, value)),
+            Commands.waitSeconds(duration))
+        .finallyDo(() -> getHID().setRumble(RumbleType.kBothRumble, 0.0));
+  }
+
   @Override
   public Trigger leftBumper(EventLoop loop) {
     return super.leftBumper(loop)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -8,6 +8,7 @@
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -269,6 +270,20 @@ public RobotContainer() {
             swerve));
 
     new Trigger(intake::beamBroken).whileTrue(new SolidColor(Color.kGreen, leds));
+
+    new Trigger(intake::beamBroken)
+        .and(RobotModeTriggers.teleop())
+        .onTrue(driverController.rumbleFor(0.25, RumbleType.kRightRumble, 1.0));
+
+    new Trigger(() -> DriverStation.getMatchTime() > 0.0 && DriverStation.getMatchTime() < 23.0)
+        .and(RobotModeTriggers.teleop())
+        .onTrue(
+            Commands.sequence(
+                driverController.rumbleFor(0.07, RumbleType.kRightRumble, 1.0),
+                Commands.waitSeconds(0.07),
+                driverController.rumbleFor(0.07, RumbleType.kRightRumble, 1.0),
+                Commands.waitSeconds(0.07),
+                driverController.rumbleFor(0.07, RumbleType.kRightRumble, 1.0)));
   }
 
   /**