diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6504519b..4ff55f21 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ 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; @@ -22,6 +23,7 @@ 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( diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java new file mode 100644 index 00000000..f693bbe9 --- /dev/null +++ b/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 robotPose; + private final CommandXboxController controller; + + public RumbleController(Supplier 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; + } +} diff --git a/src/main/java/frc/robot/commands/climber/StopClimber.java b/src/main/java/frc/robot/commands/climber/StopClimber.java index eae2a033..2811e498 100644 --- a/src/main/java/frc/robot/commands/climber/StopClimber.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/commands/sequences/CancelAll.java b/src/main/java/frc/robot/commands/sequences/CancelAll.java index 476efc4d..745b0628 100644 --- a/src/main/java/frc/robot/commands/sequences/CancelAll.java +++ b/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), diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 82aa2076..aeeb0b20 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/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; diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java new file mode 100644 index 00000000..371eae6d --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/utils/FieldZoneUtils.java b/src/main/java/frc/robot/utils/FieldZoneUtils.java new file mode 100644 index 00000000..788f3846 --- /dev/null +++ b/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 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; + } +}