fix vibration not stop on interrupt

MEFThunders-7035 · May 15, 2024 · a799d34 · a799d34
1 parent ee14ff3
commit a799d34
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/commands/VibrateControllerCommand.java b/src/main/java/frc/robot/commands/VibrateControllerCommand.java
@@ -32,6 +32,7 @@ private static Command createVibrateControllerCommand(
             new InstantCommand(() -> controller.setRumble(RumbleType.kBothRumble, 0)),
             new WaitCommand(duration));
 
-    return new RepeatForCommand(toRepeatCommand, repetitions);
+    return new RepeatForCommand(toRepeatCommand, repetitions)
+        .finallyDo(() -> controller.setRumble(RumbleType.kBothRumble, 0));
   }
 }
diff --git a/src/test/java/command_tests/controller_commands/VibrateControllerCommandTest.java b/src/test/java/command_tests/controller_commands/VibrateControllerCommandTest.java
@@ -52,6 +52,22 @@ void testItStopsVibrating() {
     assertEquals(0, controllerSim.getRumble(RumbleType.kLeftRumble), kDelta);
   }
 
+  @Test
+  void testStopOnInterrupt() {
+    commandScheduler.run();
+    SimHooks.stepTiming(kWaitTime);
+    commandScheduler.run();
+    assertEquals(0, controllerSim.getRumble(RumbleType.kLeftRumble), kDelta);
+    commandScheduler.run();
+    SimHooks.stepTiming(kWaitTime);
+    commandScheduler.run();
+    commandScheduler.run();
+    assertEquals(kIntensity, controllerSim.getRumble(RumbleType.kLeftRumble), kDelta);
+    vibrateControllerCommand.cancel();
+    commandScheduler.run();
+    assertEquals(0, controllerSim.getRumble(RumbleType.kLeftRumble), kDelta);
+  }
+
   @Test
   void testItRepeats() {
     // run command scheduler twice to end wait command and start vibration command