diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index c86bf9d4..4ff1ae1e 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/src/main/java/com/team2813/Constants.java @@ -8,6 +8,8 @@ package com.team2813; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -46,6 +48,9 @@ public final class Constants { public static final int HOOD_MOTOR_ID = 23; + public static final CommandXboxController driveController = new CommandXboxController(0); + public static final CommandXboxController operatorController = new CommandXboxController(1); + /** * Returns true if the robot is on the red alliance. * @@ -57,4 +62,14 @@ public static boolean onRed() { return DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red; } + + public static void rumble() { + driveController.setRumble(GenericHID.RumbleType.kBothRumble, 1); + operatorController.setRumble(GenericHID.RumbleType.kBothRumble, 1); + } + + public static void stopRumble() { + driveController.setRumble(GenericHID.RumbleType.kBothRumble, 0); + operatorController.setRumble(GenericHID.RumbleType.kBothRumble, 0); + } } diff --git a/src/main/java/com/team2813/Robot.java b/src/main/java/com/team2813/Robot.java index cb3626d5..602db66e 100644 --- a/src/main/java/com/team2813/Robot.java +++ b/src/main/java/com/team2813/Robot.java @@ -131,7 +131,7 @@ public void robotPeriodic() { /** This function is called once when the robot is disabled. */ @Override public void disabledInit() { - robotContainer.stopRumble(); + Constants.stopRumble(); } /** This function is called periodically when disabled. */ @@ -174,19 +174,20 @@ public void autonomousExit() { @Override public void teleopInit() {} - /** This function is called periodically during operator control. */ + /** + * This function is called periodically during operator control. + * + *

The controller rumbles for 2 seconds at the end of an inactive shift + * + *

and rumbles for 1 second when 5 seconds left in an active shift. + */ @Override public void teleopPeriodic() { - double timeLeftInCurrentPhase = HubStatusUtil.timeLeftInCurrentPhase(); - // rumble controllers 3 times if the phase is about to end - // TODO: Rework the comment, Tamir or Tom - if (timeLeftInCurrentPhase <= 3 - && (timeLeftInCurrentPhase - (int) timeLeftInCurrentPhase) > 0.7 - && timeLeftInCurrentPhase > 0) { - robotContainer.setRumbleDriver(); - robotContainer.setRumbleOperator(); + boolean shouldRumble = HubStatusUtil.shouldRumble(); + if (shouldRumble) { + Constants.rumble(); } else { - robotContainer.stopRumble(); + Constants.stopRumble(); } } diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 6d3b04ce..a9766c52 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -7,6 +7,8 @@ package com.team2813; +import static com.team2813.Constants.driveController; +import static com.team2813.Constants.operatorController; import static com.team2813.subsystems.vision.VisionConstants.aprilTagLayout; import static edu.wpi.first.units.Units.Seconds; @@ -46,7 +48,6 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.Optional; import java.util.function.BooleanSupplier; @@ -73,9 +74,6 @@ public class RobotContainer { private final Shooter shooter; private final Kicker kicker; private final Hood hood; - // Controller - private final CommandXboxController driveController = new CommandXboxController(0); - private final CommandXboxController operatorController = new CommandXboxController(1); // Dashboard inputs private final LoggedDashboardChooser autoChooser; @@ -235,6 +233,10 @@ public RobotContainer( * instantiating a {@link GenericHID} or one of its subclasses ({@link * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + * + *

See Controls + * Documentation for more details on the recommended button bindings. */ private void configureButtonBindings() { // Operator controls @@ -361,16 +363,6 @@ public void setRumbleOperator() { operatorController.setRumble(GenericHID.RumbleType.kLeftRumble, 1); } - public void setRumbleDriver() { - // TODO: test rumble values with driver - driveController.setRumble(GenericHID.RumbleType.kLeftRumble, 1); - } - - public void stopRumble() { - operatorController.setRumble(GenericHID.RumbleType.kBothRumble, 0); - driveController.setRumble(GenericHID.RumbleType.kBothRumble, 0); - } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/com/team2813/util/HubStatusUtil.java b/src/main/java/com/team2813/util/HubStatusUtil.java index ce15f049..e1d7e89c 100644 --- a/src/main/java/com/team2813/util/HubStatusUtil.java +++ b/src/main/java/com/team2813/util/HubStatusUtil.java @@ -100,4 +100,12 @@ public static double timeLeftInCurrentPhase() { return matchTimeInSeconds; } else return 0; } + + public static boolean shouldRumble() { + return shouldRumble(isHubActive(), timeLeftInCurrentPhase()); + } + + static boolean shouldRumble(boolean hubActive, double tLeft) { + return hubActive ? tLeft <= 5 && tLeft > 4 : tLeft <= 2 && tLeft > 0; + } } diff --git a/src/test/java/com/team2813/util/RobotTeleopRumbleTest.java b/src/test/java/com/team2813/util/RobotTeleopRumbleTest.java new file mode 100644 index 00000000..a39ebbfb --- /dev/null +++ b/src/test/java/com/team2813/util/RobotTeleopRumbleTest.java @@ -0,0 +1,33 @@ +package com.team2813.util; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.CsvSource; + +class RobotTeleopRumbleTest { + @ParameterizedTest + @CsvSource({ + // Active hub shift: rumble only when 4 < time <= 5. + "true,5.1,false", + "true,5.0,true", + "true,4.5,true", + "true,4.0,false", + "true,0.0,false", + // Inactive hub shift: rumble only when 0 < time <= 2. + "false,2.1,false", + "false,2.0,true", + "false,1.0,true", + "false,0.0,false", + "false,-0.1,false" + }) + void shouldRumbleMatchesTeleopPeriodicLogic( + boolean hubActive, double timeLeft, boolean expected) { + if (expected) { + assertTrue(HubStatusUtil.shouldRumble(hubActive, timeLeft)); + } else { + assertFalse(HubStatusUtil.shouldRumble(hubActive, timeLeft)); + } + } +}