First phase rumble to prevent robot from entering opponent's Barge

src/main/java/frc/robot/RobotContainer.java

@@ -15,13 +15,15 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.apriltags.ApriltagInputs;
 import frc.robot.apriltags.MockApriltag;
 import frc.robot.apriltags.TCPApriltag;
 import frc.robot.autochooser.AutoAction;
 import frc.robot.autochooser.FieldLocation;
 import frc.robot.autochooser.chooser.AutoChooser2025;
 import frc.robot.autochooser.event.RealAutoEventProvider;
+import frc.robot.commands.RumbleController;
 import frc.robot.commands.byebye.ByeByeToFwrLimit;
 import frc.robot.commands.byebye.ByeByeToRevLimit;
 import frc.robot.commands.climber.ClimbToLimit;
@@ -75,6 +77,7 @@
 import frc.robot.subsystems.swervev3.io.steer.SimSteerMotorIO;
 import frc.robot.utils.BlinkinPattern;
 import frc.robot.utils.ModulePosition;
+import frc.robot.utils.RobotMode;
 import frc.robot.utils.logging.LoggableIO;
 import frc.robot.utils.motor.Gain;
 import frc.robot.utils.motor.PID;
@@ -211,6 +214,10 @@ private void pathPlannerCommands() {
   }
 
   private void configureBindings() {
+    if (Constants.RUMBLE_CONTROLLER) {
+      new Trigger(() -> Robot.getMode() == RobotMode.TELEOP)
+          .onTrue(new RumbleController(drivetrain::getPose, controller));
+    }
     lightStrip.setDefaultCommand(
         new SetLedFromElevatorPosition(elevatorSubsystem::getStoredReefPosition, lightStrip));
     drivetrain.setDefaultCommand(

src/main/java/frc/robot/commands/RumbleController.java

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.utils.FieldZoneUtils;
+import frc.robot.utils.logging.commands.LoggableCommand;
+import java.util.function.Supplier;
+
+/* 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 RumbleController extends LoggableCommand {
+
+  private final Supplier<Pose2d> robotPose;
+  private final CommandXboxController controller;
+
+  public RumbleController(Supplier<Pose2d> robotPose, CommandXboxController controller) {
+    this.robotPose = robotPose;
+    this.controller = controller;
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    if (FieldZoneUtils.isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())) {
+        controller.setRumble(RumbleType.kBothRumble, 1);
+    } else controller.setRumble(RumbleType.kBothRumble, 0);
+  }
+
+  @Override
+  public void end(boolean interrupted) {}
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/climber/StopClimber.java

@@ -4,17 +4,20 @@
 import frc.robot.utils.logging.commands.LoggableCommand;
 
 public class StopClimber extends LoggableCommand {
-    private ClimberSubsystem climber;
-    public StopClimber(ClimberSubsystem climber) {
-        this.climber = climber;
-        addRequirements(climber);
-    }
-    @Override
-    public void initialize() {
-        climber.stopClimber();
-    }
-    @Override
-    public boolean isFinished() {
-        return true;
-    }
+  private ClimberSubsystem climber;
+
+  public StopClimber(ClimberSubsystem climber) {
+    this.climber = climber;
+    addRequirements(climber);
+  }
+
+  @Override
+  public void initialize() {
+    climber.stopClimber();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
 }

src/main/java/frc/robot/commands/sequences/CancelAll.java

@@ -10,17 +10,16 @@
 import frc.robot.subsystems.algaebyebyetilt.AlgaeByeByeTiltSubsystem;
 import frc.robot.subsystems.climber.ClimberSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
-import frc.robot.subsystems.swervev3.SwerveDrivetrain;
 import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup;
 
 /** Add your docs here. */
 public class CancelAll extends LoggableSequentialCommandGroup {
 
   public CancelAll(
-          AlgaeByeByeTiltSubsystem algaeByeByeTiltSubsystem,
-          AlgaeByeByeRollerSubsystem algaebyebyeroller,
-          ElevatorSubsystem elevatorSubsystem,
-          ClimberSubsystem climberSubsystem) {
+      AlgaeByeByeTiltSubsystem algaeByeByeTiltSubsystem,
+      AlgaeByeByeRollerSubsystem algaebyebyeroller,
+      ElevatorSubsystem elevatorSubsystem,
+      ClimberSubsystem climberSubsystem) {
     super(
         new StopClimber(climberSubsystem),
         new ByeByeAllDone(algaeByeByeTiltSubsystem, algaebyebyeroller, elevatorSubsystem),

src/main/java/frc/robot/constants/GameConstants.java

@@ -210,6 +210,7 @@ public enum Mode {
   public static final boolean HI_HI_SIMULATE_GRAVITY = false;
   public static final int MAX_VALID_TICKS_INTAKE = 15; // TODO: Change Later
   public static final int MAX_VALID_TICKS_ELEVATOR = 10; // TODO: Change Later
+  public static final boolean RUMBLE_CONTROLLER = true;
 
   public static final double ROBOT_MASS = 58.967; // In Kg, change later
   public static final double ROBOT_BUMPER_WIDTH = 0.914;

src/main/java/frc/robot/utils/Barge.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.utils;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Robot;
+
+/** Add your docs here. */
+public enum Barge {
+  RIGHT_LOWER(8.9246, 0),
+  RIGHT_HIGHER(8.9246, 4),
+  LEFT_LOWER(7.56, 0),
+  LEFT_HIGHER(7.56, 4);
+
+  private final double x;
+  private final double y;
+
+  Barge(double x, double y) {
+    this.x = x;
+    this.y = y;
+  }
+
+  public double getX() {
+    return x;
+  }
+
+  public double getY(DriverStation.Alliance AllianceColor) {
+    return AllianceColor.equals(Alliance.Blue) ? y : y + 4;
+  }
+
+  public double getY() {
+    return Robot.getAllianceColor().isPresent()
+        ? Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4
+        : -1;
+  }
+}

src/main/java/frc/robot/utils/FieldZoneUtils.java

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.utils;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Robot;
+import java.util.Optional;
+
+/** Add your docs here. */
+public class FieldZoneUtils {
+
+  public static boolean isInOppositeBarge(double x, double y) {
+    Optional<DriverStation.Alliance> al = Robot.getAllianceColor();
+    if ((x < Barge.RIGHT_HIGHER.getX()) && (x > Barge.LEFT_LOWER.getX()) && (al.isPresent())) {
+      return al.get().equals(Alliance.Red)
+          ? (y < Barge.RIGHT_HIGHER.getY())
+          : (y > Barge.RIGHT_HIGHER.getY());
+    }
+    return false;
+  }
+}