From 3dec6a74c63c2d221b8fcee2f3548a50375f766b Mon Sep 17 00:00:00 2001 From: codetoad Date: Sat, 1 Mar 2025 14:04:49 -0500 Subject: [PATCH 01/36] path changes --- src/main/java/frc/robot/RobotContainer.java | 5 ++- .../java/frc/robot/commands/SetRumble.java | 26 ++++++++++++++ .../subsystems/swervev3/SwerveDrivetrain.java | 4 +++ src/main/java/frc/robot/utils/Barge.java | 34 +++++++++++++++++++ 4 files changed, 68 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/SetRumble.java create mode 100644 src/main/java/frc/robot/utils/Barge.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7ddb263..193cddee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,17 +8,20 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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.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.commands.CancelAll; import frc.robot.commands.RollAlgae; +import frc.robot.commands.SetRumble; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; import frc.robot.commands.coral.IntakeCoral; @@ -151,7 +154,7 @@ private void configureBindings() { JoystickButton joyRight1 = new JoystickButton(joyright, 1); RobotSlide robotSlide = new RobotSlide(drivetrain, joyleft::getX, joyleft::getY); joyLeft2.whileTrue(robotSlide); - + new Trigger(()->drivetrain.isInBarge()).whileTrue(new SetRumble(controller, true)).onFalse(new SetRumble(controller, false)); controller.leftTrigger().onTrue(new PickUpCoral(elevatorSubsystem, coralSubsystem, lightStrip)); controller .povUp() diff --git a/src/main/java/frc/robot/commands/SetRumble.java b/src/main/java/frc/robot/commands/SetRumble.java new file mode 100644 index 00000000..ea8a497f --- /dev/null +++ b/src/main/java/frc/robot/commands/SetRumble.java @@ -0,0 +1,26 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class SetRumble extends LoggableCommand { + private final CommandXboxController controller; + private final boolean rumble; + + public SetRumble(CommandXboxController controller, boolean rumble) { + this.controller = controller; + this.rumble = rumble; + } + + @Override + public void initialize() { + controller.setRumble(GenericHID.RumbleType.kBothRumble, rumble ? 1: 0); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index bade2220..19e7bb1b 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -197,4 +197,8 @@ public void setFacingTarget(boolean facingTarget) { public boolean isFacingTarget() { return facingTarget; } + + public boolean isInBarge() { + return getPose().getTranslation() + } } 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..93a2fd83 --- /dev/null +++ b/src/main/java/frc/robot/utils/Barge.java @@ -0,0 +1,34 @@ +package frc.robot.utils; + +public enum Barge { + RED(7.56,1.3646 ,0,4), + BLUE(7.56,1.3646 ,4,8); + + private final double x1; + private final double width; + private final double y1; + private final double height; + + Barge(double x1, double width, double y1, double height) { + this.x1 = x1; + this.width = width; + this.y1 = y1; + this.height = height; + } + + public double getX1() { + return x1; + } + + public double getWidth() { + return width; + } + + public double getY1() { + return y1; + } + + public double getHeight() { + return height; + } +} From 2af2c92aa91792beb88ccd4c955a3f6909d18128 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 13 Mar 2025 19:31:08 -0400 Subject: [PATCH 02/36] broken --- .../subsystems/swervev3/SwerveDrivetrain.java | 12 +++++++- .../java/frc/robot/utils/BargePoints.java | 28 +++++++++++++++++++ 2 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/utils/BargePoints.java diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 19e7bb1b..89f42853 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -15,6 +15,8 @@ import frc.robot.subsystems.swervev3.bags.OdometryMeasurement; import frc.robot.subsystems.swervev3.estimation.PoseEstimator; import frc.robot.subsystems.swervev3.io.SwerveModule; +import frc.robot.utils.Barge; +import frc.robot.utils.BargePoints; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; @@ -199,6 +201,14 @@ public boolean isFacingTarget() { } public boolean isInBarge() { - return getPose().getTranslation() + if(BargePoints.BLUEHIGHER.getx() > getPose().getTranslation().getX() && getPose().getTranslation().getX() > BargePoints.REDLOWER.getx()){//switch to Just in barge + if (){//switch to red/blue + return true; + } else if(true){ + return true; + } + + } + return false; } } diff --git a/src/main/java/frc/robot/utils/BargePoints.java b/src/main/java/frc/robot/utils/BargePoints.java new file mode 100644 index 00000000..a06668e0 --- /dev/null +++ b/src/main/java/frc/robot/utils/BargePoints.java @@ -0,0 +1,28 @@ +// 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; + +/** Add your docs here. */ +public enum BargePoints { + BLUELOWER(8.9246,0), + BLUEHIGHER(8.9246,4), + REDLOWER(7.56,4), + REDHIGHER(7.56,8) + ; + + + private final double x; + private final double y; + BargePoints(double x, double y){ + this.x = x; + this.y = y; + } + public double getY(){ + return y; + } + public double getx(){ + return x; + } +} From 88683d0a111ba315120131c832f0969399648278 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 13 Mar 2025 19:33:26 -0400 Subject: [PATCH 03/36] merge errors --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ce25c68..3beed155 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -213,7 +213,7 @@ private void configureBindings() { RobotSlide robotSlide = new RobotSlide(drivetrain, joyleft::getX, joyleft::getY); joyLeft2.whileTrue(robotSlide); - controller.leftTrigger().onTrue(new PickUpCoral(elevatorSubsystem, coralSubsystem, lightStrip)); + controller.leftTrigger().onTrue(new PickUpCoral(elevatorSubsystem, byebyeTilt, byebyeRoller, coralSubsystem, lightStrip)); controller .povUp() .onTrue( From c421942fafc1701f20ca1123fc87b0d84b233d3b Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 13 Mar 2025 20:28:25 -0400 Subject: [PATCH 04/36] update --- src/main/java/frc/robot/RobotContainer.java | 10 ++-- .../frc/robot/commands/RumbleController.java | 46 ++++++++++++++++++ .../java/frc/robot/commands/SetRumble.java | 29 ++++++----- .../subsystems/swervev3/SwerveDrivetrain.java | 21 ++++---- src/main/java/frc/robot/utils/Barge.java | 48 +++++++++---------- .../java/frc/robot/utils/BargePoints.java | 35 +++++++------- 6 files changed, 120 insertions(+), 69 deletions(-) create mode 100644 src/main/java/frc/robot/commands/RumbleController.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3beed155..566329a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,14 +8,11 @@ import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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; @@ -24,7 +21,6 @@ import frc.robot.autochooser.chooser.AutoChooser2025; import frc.robot.autochooser.event.RealAutoEventProvider; import frc.robot.commands.RollAlgae; -import frc.robot.commands.SetRumble; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; import frc.robot.commands.coral.IntakeCoral; @@ -213,7 +209,11 @@ private void configureBindings() { RobotSlide robotSlide = new RobotSlide(drivetrain, joyleft::getX, joyleft::getY); joyLeft2.whileTrue(robotSlide); - controller.leftTrigger().onTrue(new PickUpCoral(elevatorSubsystem, byebyeTilt, byebyeRoller, coralSubsystem, lightStrip)); + controller + .leftTrigger() + .onTrue( + new PickUpCoral( + elevatorSubsystem, byebyeTilt, byebyeRoller, coralSubsystem, lightStrip)); controller .povUp() .onTrue( 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..3f278528 --- /dev/null +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -0,0 +1,46 @@ +// 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.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableCommand; + +import java.util.function.DoubleSupplier; + +/* 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 DoubleSupplier X; + private final DoubleSupplier Y; + private final SwerveDrivetrain swerve; + private final XboxController controller; + + public RumbleController(SwerveDrivetrain swerve, DoubleSupplier X, DoubleSupplier Y, XboxController controller) { + this.Y = Y; + this.X = X; + this.swerve = swerve; + this.controller = controller; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + if(swerve.isInBarge(X.getAsDouble(),Y.getAsDouble())){ +controller.setRumble(RumbleType.kBothRumble, 2); + }; + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SetRumble.java b/src/main/java/frc/robot/commands/SetRumble.java index ea8a497f..229a2dfc 100644 --- a/src/main/java/frc/robot/commands/SetRumble.java +++ b/src/main/java/frc/robot/commands/SetRumble.java @@ -1,26 +1,25 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.utils.logging.commands.LoggableCommand; public class SetRumble extends LoggableCommand { - private final CommandXboxController controller; - private final boolean rumble; + private final CommandXboxController controller; + private final boolean rumble; - public SetRumble(CommandXboxController controller, boolean rumble) { - this.controller = controller; - this.rumble = rumble; - } + public SetRumble(CommandXboxController controller, boolean rumble) { + this.controller = controller; + this.rumble = rumble; + } - @Override - public void initialize() { - controller.setRumble(GenericHID.RumbleType.kBothRumble, rumble ? 1: 0); - } + @Override + public void initialize() { + controller.setRumble(GenericHID.RumbleType.kBothRumble, rumble ? 1 : 0); + } - @Override - public boolean isFinished() { - return true; - } + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index b546f367..25fcc230 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -14,7 +14,9 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.apriltags.ApriltagInputs; import frc.robot.constants.Constants; import frc.robot.subsystems.gyro.GyroIO; @@ -22,11 +24,12 @@ import frc.robot.subsystems.swervev3.bags.OdometryMeasurement; import frc.robot.subsystems.swervev3.estimation.PoseEstimator; import frc.robot.subsystems.swervev3.io.SwerveModule; -import frc.robot.utils.Barge; +import frc.robot.subsystems.swervev3.vision.DistanceVisionTruster; import frc.robot.utils.BargePoints; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; +import java.util.Optional; import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { @@ -247,14 +250,16 @@ public boolean isFacingTarget() { return facingTarget; } - public boolean isInBarge() { - if(BargePoints.BLUEHIGHER.getx() > getPose().getTranslation().getX() && getPose().getTranslation().getX() > BargePoints.REDLOWER.getx()){//switch to Just in barge - if (){//switch to red/blue - return true; - } else if(true){ - return true; + public boolean isInBarge(double Y, double X) { + Optional al = Robot.getAllianceColor(); + if (BargePoints.BLUE_HIGHER.getX() > X && X > BargePoints.RED_LOWER.getX()) { + if (al.isPresent()) { + if (al.get().equals(Alliance.Red)) { + return (BargePoints.BLUE_HIGHER.getY() > Y && BargePoints.BLUE_LOWER.getY() < Y); + } else { + return BargePoints.RED_HIGHER.getY() > Y && BargePoints.RED_LOWER.getY() < Y; } - + } } return false; } diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index 93a2fd83..73416825 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -1,34 +1,34 @@ package frc.robot.utils; public enum Barge { - RED(7.56,1.3646 ,0,4), - BLUE(7.56,1.3646 ,4,8); + RED(7.56, 1.3646, 0, 4), + BLUE(7.56, 1.3646, 4, 8); - private final double x1; - private final double width; - private final double y1; - private final double height; + private final double x1; + private final double width; + private final double y1; + private final double height; - Barge(double x1, double width, double y1, double height) { - this.x1 = x1; - this.width = width; - this.y1 = y1; - this.height = height; - } + Barge(double x1, double width, double y1, double height) { + this.x1 = x1; + this.width = width; + this.y1 = y1; + this.height = height; + } - public double getX1() { - return x1; - } + public double getX1() { + return x1; + } - public double getWidth() { - return width; - } + public double getWidth() { + return width; + } - public double getY1() { - return y1; - } + public double getY1() { + return y1; + } - public double getHeight() { - return height; - } + public double getHeight() { + return height; + } } diff --git a/src/main/java/frc/robot/utils/BargePoints.java b/src/main/java/frc/robot/utils/BargePoints.java index a06668e0..b972fa4a 100644 --- a/src/main/java/frc/robot/utils/BargePoints.java +++ b/src/main/java/frc/robot/utils/BargePoints.java @@ -6,23 +6,24 @@ /** Add your docs here. */ public enum BargePoints { - BLUELOWER(8.9246,0), - BLUEHIGHER(8.9246,4), - REDLOWER(7.56,4), - REDHIGHER(7.56,8) - ; + BLUE_LOWER(8.9246, 0), + BLUE_HIGHER(8.9246, 4), + RED_LOWER(7.56, 4), + RED_HIGHER(7.56, 8); + private final double x; + private final double y; - private final double x; - private final double y; - BargePoints(double x, double y){ - this.x = x; - this.y = y; - } - public double getY(){ - return y; - } - public double getx(){ - return x; - } + BargePoints(double x, double y) { + this.x = x; + this.y = y; + } + + public double getY() { + return y; + } + + public double getX() { + return x; + } } From 13931bf4ff4aba7e90f4a216b3b402d8c2260e36 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Fri, 14 Mar 2025 18:02:45 -0400 Subject: [PATCH 05/36] update --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 13 ++++++- .../frc/robot/commands/RumbleController.java | 38 +++++++++++++------ .../subsystems/swervev3/SwerveDrivetrain.java | 14 +------ src/main/java/frc/robot/utils/RobotMode.java | 2 +- 5 files changed, 41 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f6694c38..a562d67d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -189,4 +189,5 @@ public static Diagnostics getDiagnostics() { public static Optional getAllianceColor() { return allianceColor; } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 566329a2..857c0c24 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,9 @@ package frc.robot; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + import com.pathplanner.lib.auto.NamedCommands; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; @@ -13,6 +16,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; @@ -21,6 +25,7 @@ import frc.robot.autochooser.chooser.AutoChooser2025; import frc.robot.autochooser.event.RealAutoEventProvider; import frc.robot.commands.RollAlgae; +import frc.robot.commands.RumbleController; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; import frc.robot.commands.coral.IntakeCoral; @@ -91,6 +96,7 @@ import frc.robot.subsystems.swervev3.io.steer.MockSteerMotorIO; 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; @@ -250,7 +256,12 @@ private void configureBindings() { // SmartDashboard.putData("Climber reset", new ResetClimber(climber)); // SmartDashboard.putData("Climber stop", new CloseClimber(climber)); } - } + BooleanSupplier suplier = new BooleanSupplier(){ + public boolean getAsBoolean(){ + return Robot.getMode() == RobotMode.TELEOP; + } + }; + new Trigger(suplier).onTrue(new RumbleController(drivetrain::getPose, controller)); public Command getAutonomousCommand() { return autoChooser.getAutoCommand(); diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 3f278528..e0ffd459 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -5,33 +5,47 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.Robot; +import frc.robot.utils.BargePoints; import frc.robot.utils.logging.commands.LoggableCommand; -import java.util.function.DoubleSupplier; +import java.util.Optional; +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 DoubleSupplier X; - private final DoubleSupplier Y; - private final SwerveDrivetrain swerve; - private final XboxController controller; + private final Supplier pose2D; + private final CommandXboxController controller; - public RumbleController(SwerveDrivetrain swerve, DoubleSupplier X, DoubleSupplier Y, XboxController controller) { - this.Y = Y; - this.X = X; - this.swerve = swerve; + public RumbleController(Supplier pose2D, CommandXboxController controller) { + this.pose2D = pose2D; this.controller = controller; } - +public boolean isInBarge(double Y, double X) { + Optional al = Robot.getAllianceColor(); + if (BargePoints.BLUE_HIGHER.getX() > X && X > BargePoints.RED_LOWER.getX()) { + if (al.isPresent()) { + if (al.get().equals(Alliance.Red)) { + return (BargePoints.BLUE_HIGHER.getY() > Y && BargePoints.BLUE_LOWER.getY() < Y); + } else { + return BargePoints.RED_HIGHER.getY() > Y && BargePoints.RED_LOWER.getY() < Y; + } + } + } + return false; + } @Override public void initialize() {} @Override public void execute() { - if(swerve.isInBarge(X.getAsDouble(),Y.getAsDouble())){ + if(isInBarge(pose2D.get().getX(),pose2D.get().getY())){ controller.setRumble(RumbleType.kBothRumble, 2); }; } diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 25fcc230..09250d05 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -250,19 +250,7 @@ public boolean isFacingTarget() { return facingTarget; } - public boolean isInBarge(double Y, double X) { - Optional al = Robot.getAllianceColor(); - if (BargePoints.BLUE_HIGHER.getX() > X && X > BargePoints.RED_LOWER.getX()) { - if (al.isPresent()) { - if (al.get().equals(Alliance.Red)) { - return (BargePoints.BLUE_HIGHER.getY() > Y && BargePoints.BLUE_LOWER.getY() < Y); - } else { - return BargePoints.RED_HIGHER.getY() > Y && BargePoints.RED_LOWER.getY() < Y; - } - } - } - return false; - } + public void setVisionBaseSTD(Vector std) { ((DistanceVisionTruster) poseEstimator.getPoseManager().getVisionTruster()).setInitialSTD(std); diff --git a/src/main/java/frc/robot/utils/RobotMode.java b/src/main/java/frc/robot/utils/RobotMode.java index d8bb1246..b89c91d1 100644 --- a/src/main/java/frc/robot/utils/RobotMode.java +++ b/src/main/java/frc/robot/utils/RobotMode.java @@ -5,5 +5,5 @@ public enum RobotMode { TELEOP, AUTONOMOUS, TEST, - SIMULATION + SIMULATION; } From 566751c414d83ae1755a4e1b584f43c0b3ba0499 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Fri, 14 Mar 2025 18:24:34 -0400 Subject: [PATCH 06/36] UPDATE --- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 16 ++++++++-------- .../frc/robot/commands/RumbleController.java | 15 ++++++++------- .../subsystems/swervev3/SwerveDrivetrain.java | 6 ------ 4 files changed, 16 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a562d67d..f6694c38 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -189,5 +189,4 @@ public static Diagnostics getDiagnostics() { public static Optional getAllianceColor() { return allianceColor; } - } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 857c0c24..42ffd114 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,9 +4,6 @@ package frc.robot; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; - import com.pathplanner.lib.auto.NamedCommands; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; @@ -100,6 +97,7 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; +import java.util.function.BooleanSupplier; public class RobotContainer { private AutoChooser2025 autoChooser; @@ -256,12 +254,14 @@ private void configureBindings() { // SmartDashboard.putData("Climber reset", new ResetClimber(climber)); // SmartDashboard.putData("Climber stop", new CloseClimber(climber)); } - BooleanSupplier suplier = new BooleanSupplier(){ - public boolean getAsBoolean(){ + BooleanSupplier suplier = + new BooleanSupplier() { + public boolean getAsBoolean() { return Robot.getMode() == RobotMode.TELEOP; - } - }; - new Trigger(suplier).onTrue(new RumbleController(drivetrain::getPose, controller)); + } + }; + new Trigger(suplier).onTrue(new RumbleController(drivetrain::getPose, controller)); + } public Command getAutonomousCommand() { return autoChooser.getAutoCommand(); diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index e0ffd459..2c904e1c 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -4,16 +4,14 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot; import frc.robot.utils.BargePoints; import frc.robot.utils.logging.commands.LoggableCommand; - import java.util.Optional; import java.util.function.Supplier; @@ -27,7 +25,8 @@ public RumbleController(Supplier pose2D, CommandXboxController controlle this.pose2D = pose2D; this.controller = controller; } -public boolean isInBarge(double Y, double X) { + + public boolean isInBarge(double Y, double X) { Optional al = Robot.getAllianceColor(); if (BargePoints.BLUE_HIGHER.getX() > X && X > BargePoints.RED_LOWER.getX()) { if (al.isPresent()) { @@ -40,14 +39,16 @@ public boolean isInBarge(double Y, double X) { } return false; } + @Override public void initialize() {} @Override public void execute() { - if(isInBarge(pose2D.get().getX(),pose2D.get().getY())){ -controller.setRumble(RumbleType.kBothRumble, 2); - }; + if (isInBarge(pose2D.get().getX(), pose2D.get().getY())) { + controller.setRumble(RumbleType.kBothRumble, 2); + } + ; } @Override diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 09250d05..fd6d92fb 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -14,9 +14,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; import frc.robot.apriltags.ApriltagInputs; import frc.robot.constants.Constants; import frc.robot.subsystems.gyro.GyroIO; @@ -25,11 +23,9 @@ import frc.robot.subsystems.swervev3.estimation.PoseEstimator; import frc.robot.subsystems.swervev3.io.SwerveModule; import frc.robot.subsystems.swervev3.vision.DistanceVisionTruster; -import frc.robot.utils.BargePoints; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; -import java.util.Optional; import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { @@ -250,8 +246,6 @@ public boolean isFacingTarget() { return facingTarget; } - - public void setVisionBaseSTD(Vector std) { ((DistanceVisionTruster) poseEstimator.getPoseManager().getVisionTruster()).setInitialSTD(std); } From c8dd298737ed62bf1a21feb9a8e1c5409a18c86e Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Fri, 14 Mar 2025 18:37:30 -0400 Subject: [PATCH 07/36] updated to use lamda --- src/main/java/frc/robot/RobotContainer.java | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 42ffd114..93225601 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -254,13 +254,8 @@ private void configureBindings() { // SmartDashboard.putData("Climber reset", new ResetClimber(climber)); // SmartDashboard.putData("Climber stop", new CloseClimber(climber)); } - BooleanSupplier suplier = - new BooleanSupplier() { - public boolean getAsBoolean() { - return Robot.getMode() == RobotMode.TELEOP; - } - }; - new Trigger(suplier).onTrue(new RumbleController(drivetrain::getPose, controller)); + + new Trigger(() -> Robot.getMode() == RobotMode.TELEOP).onTrue(new RumbleController(drivetrain::getPose, controller)); } public Command getAutonomousCommand() { From 5afb8456423bb91ff9a11818e5e54b2a6ca0b6c8 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Fri, 14 Mar 2025 22:42:42 -0400 Subject: [PATCH 08/36] added two square collison? --- .../java/frc/robot/commands/RumbleController.java | 14 ++++++++++++-- .../subsystems/swervev3/SwerveDrivetrain.java | 3 +++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 2c904e1c..74b2b02a 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot; +import frc.robot.constants.Constants; import frc.robot.utils.BargePoints; import frc.robot.utils.logging.commands.LoggableCommand; import java.util.Optional; @@ -26,6 +27,13 @@ public RumbleController(Supplier pose2D, CommandXboxController controlle this.controller = controller; } + public double findXPointOfCenterX(double x,double degreesFromBaseDegrees){ + return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)+x)*Constants.DRIVE_BASE_LENGTH); + } + public double findYPointOfCenterY(double y, double degreesFromBaseDegrees){ + return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)+y)*Constants.DRIVE_BASE_WIDTH); + } + public boolean isInBarge(double Y, double X) { Optional al = Robot.getAllianceColor(); if (BargePoints.BLUE_HIGHER.getX() > X && X > BargePoints.RED_LOWER.getX()) { @@ -45,10 +53,12 @@ public void initialize() {} @Override public void execute() { - if (isInBarge(pose2D.get().getX(), pose2D.get().getY())) { + for(int i = 0; i > 360; i += 90){ + if (isInBarge(findXPointOfCenterX(pose2D.get().getX(), i), findYPointOfCenterY(pose2D.get().getY(),i))) { controller.setRumble(RumbleType.kBothRumble, 2); + break; + } } - ; } @Override diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index fd6d92fb..6b3fa86f 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -26,6 +26,9 @@ import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; + +import java.lang.reflect.Array; + import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { From 5e4228591d0026e4d1915519360332b08c3bbe1b Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Fri, 14 Mar 2025 23:13:00 -0400 Subject: [PATCH 09/36] update --- src/main/java/frc/robot/commands/RumbleController.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 74b2b02a..ceb686e0 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -28,10 +28,10 @@ public RumbleController(Supplier pose2D, CommandXboxController controlle } public double findXPointOfCenterX(double x,double degreesFromBaseDegrees){ - return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)+x)*Constants.DRIVE_BASE_LENGTH); + return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)+(x/Constants.DRIVE_BASE_LENGTH))*Constants.DRIVE_BASE_LENGTH); } public double findYPointOfCenterY(double y, double degreesFromBaseDegrees){ - return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)+y)*Constants.DRIVE_BASE_WIDTH); + return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)+(y/Constants.DRIVE_BASE_WIDTH))*Constants.DRIVE_BASE_WIDTH); } public boolean isInBarge(double Y, double X) { From 2796432b6b6f830369991acce72d8ee613d80f89 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 15 Mar 2025 07:26:03 -0400 Subject: [PATCH 10/36] update --- src/main/java/frc/robot/commands/RumbleController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index ceb686e0..39c7883e 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -53,7 +53,7 @@ public void initialize() {} @Override public void execute() { - for(int i = 0; i > 360; i += 90){ + for(int i = 45; i > 360; i += 90){ if (isInBarge(findXPointOfCenterX(pose2D.get().getX(), i), findYPointOfCenterY(pose2D.get().getY(),i))) { controller.setRumble(RumbleType.kBothRumble, 2); break; From 895ea49d1e740631e8470fc2802ca98e1a2077ab Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 15 Mar 2025 10:06:43 -0400 Subject: [PATCH 11/36] update --- src/main/java/frc/robot/commands/RumbleController.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 39c7883e..e975750a 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -28,10 +28,10 @@ public RumbleController(Supplier pose2D, CommandXboxController controlle } public double findXPointOfCenterX(double x,double degreesFromBaseDegrees){ - return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)+(x/Constants.DRIVE_BASE_LENGTH))*Constants.DRIVE_BASE_LENGTH); + return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)*Constants.DRIVE_BASE_LENGTH)+x); } public double findYPointOfCenterY(double y, double degreesFromBaseDegrees){ - return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)+(y/Constants.DRIVE_BASE_WIDTH))*Constants.DRIVE_BASE_WIDTH); + return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)*Constants.DRIVE_BASE_WIDTH)+y); } public boolean isInBarge(double Y, double X) { From f0df905161cab49b96a47fe4e0aa21a3ad3fa8d6 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Sun, 16 Mar 2025 18:32:05 -0400 Subject: [PATCH 12/36] Fixed a few things --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/commands/RumbleController.java | 39 +++++++------------ .../subsystems/swervev3/SwerveDrivetrain.java | 3 -- 3 files changed, 15 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 93225601..d5eecb6f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -97,7 +97,6 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; -import java.util.function.BooleanSupplier; public class RobotContainer { private AutoChooser2025 autoChooser; @@ -255,7 +254,8 @@ private void configureBindings() { // SmartDashboard.putData("Climber stop", new CloseClimber(climber)); } - new Trigger(() -> Robot.getMode() == RobotMode.TELEOP).onTrue(new RumbleController(drivetrain::getPose, controller)); + new Trigger(() -> Robot.getMode() == RobotMode.TELEOP) + .onTrue(new RumbleController(drivetrain::getPose, controller)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index e975750a..0dd0ca7f 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot; -import frc.robot.constants.Constants; import frc.robot.utils.BargePoints; import frc.robot.utils.logging.commands.LoggableCommand; import java.util.Optional; @@ -19,31 +18,22 @@ /* 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; + private final Supplier robotPose; private final CommandXboxController controller; - public RumbleController(Supplier pose2D, CommandXboxController controller) { - this.pose2D = pose2D; + public RumbleController(Supplier robotPose, CommandXboxController controller) { + this.robotPose = robotPose; this.controller = controller; } - public double findXPointOfCenterX(double x,double degreesFromBaseDegrees){ - return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)*Constants.DRIVE_BASE_LENGTH)+x); - } - public double findYPointOfCenterY(double y, double degreesFromBaseDegrees){ - return (Math.cos((pose2D.get().getTranslation().getAngle().getDegrees()+degreesFromBaseDegrees)*Constants.DRIVE_BASE_WIDTH)+y); - } - - public boolean isInBarge(double Y, double X) { + public static boolean isInBarge(double x, double y) { Optional al = Robot.getAllianceColor(); - if (BargePoints.BLUE_HIGHER.getX() > X && X > BargePoints.RED_LOWER.getX()) { - if (al.isPresent()) { - if (al.get().equals(Alliance.Red)) { - return (BargePoints.BLUE_HIGHER.getY() > Y && BargePoints.BLUE_LOWER.getY() < Y); - } else { - return BargePoints.RED_HIGHER.getY() > Y && BargePoints.RED_LOWER.getY() < Y; - } - } + if ((BargePoints.BLUE_HIGHER.getX() > x) + && (BargePoints.RED_LOWER.getX() < x) + && (al.isPresent())) { + return al.get().equals(Alliance.Red) + ? (BargePoints.BLUE_HIGHER.getY() > y && BargePoints.BLUE_LOWER.getY() < y) + : (BargePoints.RED_HIGHER.getY() > y && BargePoints.RED_LOWER.getY() < y); } return false; } @@ -53,12 +43,9 @@ public void initialize() {} @Override public void execute() { - for(int i = 45; i > 360; i += 90){ - if (isInBarge(findXPointOfCenterX(pose2D.get().getX(), i), findYPointOfCenterY(pose2D.get().getY(),i))) { - controller.setRumble(RumbleType.kBothRumble, 2); - break; - } - } + if (isInBarge(robotPose.get().getX(), robotPose.get().getY())) + controller.setRumble(RumbleType.kBothRumble, 0.5); + else controller.setRumble(RumbleType.kBothRumble, 0); } @Override diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 6b3fa86f..fd6d92fb 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -26,9 +26,6 @@ import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; - -import java.lang.reflect.Array; - import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { From 7b5eabb2ff5d935f583e01d1fb9388a6d66d90a0 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Sun, 16 Mar 2025 18:52:23 -0400 Subject: [PATCH 13/36] removed unnecesary symbol --- src/main/java/frc/robot/utils/RobotMode.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/RobotMode.java b/src/main/java/frc/robot/utils/RobotMode.java index b89c91d1..d8bb1246 100644 --- a/src/main/java/frc/robot/utils/RobotMode.java +++ b/src/main/java/frc/robot/utils/RobotMode.java @@ -5,5 +5,5 @@ public enum RobotMode { TELEOP, AUTONOMOUS, TEST, - SIMULATION; + SIMULATION } From 099f5d910f0fb7dba6c9bd7f77a225d579fa083d Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Mon, 17 Mar 2025 12:03:38 -0400 Subject: [PATCH 14/36] renamed method to be clearer --- src/main/java/frc/robot/commands/RumbleController.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 0dd0ca7f..1be6fc57 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -26,7 +26,7 @@ public RumbleController(Supplier robotPose, CommandXboxController contro this.controller = controller; } - public static boolean isInBarge(double x, double y) { + public static boolean isInOppositeBarge(double x, double y) { Optional al = Robot.getAllianceColor(); if ((BargePoints.BLUE_HIGHER.getX() > x) && (BargePoints.RED_LOWER.getX() < x) @@ -43,7 +43,7 @@ public void initialize() {} @Override public void execute() { - if (isInBarge(robotPose.get().getX(), robotPose.get().getY())) + if (isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())) controller.setRumble(RumbleType.kBothRumble, 0.5); else controller.setRumble(RumbleType.kBothRumble, 0); } From aa7a13ef3e65ab18e5d731cdb2775662bfa5692a Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Mon, 17 Mar 2025 19:15:49 -0400 Subject: [PATCH 15/36] renamed variable --- src/main/java/frc/robot/utils/Barge.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index 73416825..d16bc6b5 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -4,28 +4,28 @@ public enum Barge { RED(7.56, 1.3646, 0, 4), BLUE(7.56, 1.3646, 4, 8); - private final double x1; + private final double x; private final double width; - private final double y1; + private final double y; private final double height; - Barge(double x1, double width, double y1, double height) { - this.x1 = x1; + Barge(double x, double width, double y, double height) { + this.x = x; this.width = width; - this.y1 = y1; + this.y = y; this.height = height; } - public double getX1() { - return x1; + public double getX() { + return x; } public double getWidth() { return width; } - public double getY1() { - return y1; + public double getY() { + return y; } public double getHeight() { From 55bb7540d2bb9084a4e016e3f845e2c9b6919837 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Wed, 19 Mar 2025 08:14:18 -0400 Subject: [PATCH 16/36] Removed unused classes --- .../java/frc/robot/commands/SetRumble.java | 25 --------------- src/main/java/frc/robot/utils/Barge.java | 31 ++++++++----------- .../java/frc/robot/utils/BargePoints.java | 29 ----------------- 3 files changed, 13 insertions(+), 72 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/SetRumble.java delete mode 100644 src/main/java/frc/robot/utils/BargePoints.java diff --git a/src/main/java/frc/robot/commands/SetRumble.java b/src/main/java/frc/robot/commands/SetRumble.java deleted file mode 100644 index 229a2dfc..00000000 --- a/src/main/java/frc/robot/commands/SetRumble.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.utils.logging.commands.LoggableCommand; - -public class SetRumble extends LoggableCommand { - private final CommandXboxController controller; - private final boolean rumble; - - public SetRumble(CommandXboxController controller, boolean rumble) { - this.controller = controller; - this.rumble = rumble; - } - - @Override - public void initialize() { - controller.setRumble(GenericHID.RumbleType.kBothRumble, rumble ? 1 : 0); - } - - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index d16bc6b5..b972fa4a 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -1,34 +1,29 @@ +// 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; -public enum Barge { - RED(7.56, 1.3646, 0, 4), - BLUE(7.56, 1.3646, 4, 8); +/** Add your docs here. */ +public enum BargePoints { + BLUE_LOWER(8.9246, 0), + BLUE_HIGHER(8.9246, 4), + RED_LOWER(7.56, 4), + RED_HIGHER(7.56, 8); private final double x; - private final double width; private final double y; - private final double height; - Barge(double x, double width, double y, double height) { + BargePoints(double x, double y) { this.x = x; - this.width = width; this.y = y; - this.height = height; - } - - public double getX() { - return x; - } - - public double getWidth() { - return width; } public double getY() { return y; } - public double getHeight() { - return height; + public double getX() { + return x; } } diff --git a/src/main/java/frc/robot/utils/BargePoints.java b/src/main/java/frc/robot/utils/BargePoints.java deleted file mode 100644 index b972fa4a..00000000 --- a/src/main/java/frc/robot/utils/BargePoints.java +++ /dev/null @@ -1,29 +0,0 @@ -// 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; - -/** Add your docs here. */ -public enum BargePoints { - BLUE_LOWER(8.9246, 0), - BLUE_HIGHER(8.9246, 4), - RED_LOWER(7.56, 4), - RED_HIGHER(7.56, 8); - - private final double x; - private final double y; - - BargePoints(double x, double y) { - this.x = x; - this.y = y; - } - - public double getY() { - return y; - } - - public double getX() { - return x; - } -} From 0dd4b00cc7ff5f5eccaf78fa45b78c1228cd7304 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Wed, 19 Mar 2025 08:15:59 -0400 Subject: [PATCH 17/36] idk why these didn't commit aswell --- src/main/java/frc/robot/commands/RumbleController.java | 10 +++++----- src/main/java/frc/robot/utils/Barge.java | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 1be6fc57..1f081fe0 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot; -import frc.robot.utils.BargePoints; +import frc.robot.utils.Barge; import frc.robot.utils.logging.commands.LoggableCommand; import java.util.Optional; import java.util.function.Supplier; @@ -28,12 +28,12 @@ public RumbleController(Supplier robotPose, CommandXboxController contro public static boolean isInOppositeBarge(double x, double y) { Optional al = Robot.getAllianceColor(); - if ((BargePoints.BLUE_HIGHER.getX() > x) - && (BargePoints.RED_LOWER.getX() < x) + if ((Barge.BLUE_HIGHER.getX() > x) + && (Barge.RED_LOWER.getX() < x) && (al.isPresent())) { return al.get().equals(Alliance.Red) - ? (BargePoints.BLUE_HIGHER.getY() > y && BargePoints.BLUE_LOWER.getY() < y) - : (BargePoints.RED_HIGHER.getY() > y && BargePoints.RED_LOWER.getY() < y); + ? (Barge.BLUE_HIGHER.getY() > y && Barge.BLUE_LOWER.getY() < y) + : (Barge.RED_HIGHER.getY() > y && Barge.RED_LOWER.getY() < y); } return false; } diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index b972fa4a..2f43f716 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -5,7 +5,7 @@ package frc.robot.utils; /** Add your docs here. */ -public enum BargePoints { +public enum Barge { BLUE_LOWER(8.9246, 0), BLUE_HIGHER(8.9246, 4), RED_LOWER(7.56, 4), @@ -14,7 +14,7 @@ public enum BargePoints { private final double x; private final double y; - BargePoints(double x, double y) { + Barge(double x, double y) { this.x = x; this.y = y; } From 5a323c5501d73c85b84921c2a9900aaaa5fe85e6 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Wed, 19 Mar 2025 19:38:47 -0400 Subject: [PATCH 18/36] IT WORKKKKKSKSKSKKSKSKS --- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../frc/robot/commands/RumbleController.java | 16 +++++++++++----- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d5eecb6f..7e63d98b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -253,7 +253,8 @@ private void configureBindings() { // SmartDashboard.putData("Climber reset", new ResetClimber(climber)); // SmartDashboard.putData("Climber stop", new CloseClimber(climber)); } - + SmartDashboard.putNumber("X", 0.0); + SmartDashboard.putNumber("Y", 0.0); new Trigger(() -> Robot.getMode() == RobotMode.TELEOP) .onTrue(new RumbleController(drivetrain::getPose, controller)); } diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 0dd0ca7f..7cfcb224 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot; import frc.robot.utils.BargePoints; @@ -28,12 +29,12 @@ public RumbleController(Supplier robotPose, CommandXboxController contro public static boolean isInBarge(double x, double y) { Optional al = Robot.getAllianceColor(); - if ((BargePoints.BLUE_HIGHER.getX() > x) - && (BargePoints.RED_LOWER.getX() < x) + if ((x < BargePoints.BLUE_HIGHER.getX()) + && (x > BargePoints.RED_LOWER.getX()) && (al.isPresent())) { return al.get().equals(Alliance.Red) - ? (BargePoints.BLUE_HIGHER.getY() > y && BargePoints.BLUE_LOWER.getY() < y) - : (BargePoints.RED_HIGHER.getY() > y && BargePoints.RED_LOWER.getY() < y); + ? (y < BargePoints.BLUE_HIGHER.getY()) + : (y > BargePoints.RED_LOWER.getY()); } return false; } @@ -43,9 +44,14 @@ public void initialize() {} @Override public void execute() { - if (isInBarge(robotPose.get().getX(), robotPose.get().getY())) + if (isInBarge(SmartDashboard.getNumber("X", 0.0), SmartDashboard.getNumber("Y", 0.0))) controller.setRumble(RumbleType.kBothRumble, 0.5); else controller.setRumble(RumbleType.kBothRumble, 0); + + System.out.println("X" + SmartDashboard.getNumber("X", 0.0)); + System.out.println("Y" + SmartDashboard.getNumber("Y", 0.0)); + System.out.println(Robot.getAllianceColor()); + // robotPose.get().getX(), robotPose.get().getY() } @Override From f6751f3aa376c99283e42c8fcebb3d48fe156c78 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Wed, 19 Mar 2025 19:41:39 -0400 Subject: [PATCH 19/36] error con merge --- src/main/java/frc/robot/commands/RumbleController.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index fb169f2e..bb654de1 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot; import frc.robot.utils.Barge; @@ -29,12 +28,10 @@ public RumbleController(Supplier robotPose, CommandXboxController contro public static boolean isInOppositeBarge(double x, double y) { Optional al = Robot.getAllianceColor(); - if ((x < BargePoints.BLUE_HIGHER.getX()) - && (x > BargePoints.RED_LOWER.getX()) - && (al.isPresent())) { + if ((x < Barge.BLUE_HIGHER.getX()) && (x > Barge.RED_LOWER.getX()) && (al.isPresent())) { return al.get().equals(Alliance.Red) - ? (y < BargePoints.BLUE_HIGHER.getY()) - : (y > BargePoints.RED_LOWER.getY()); + ? (y < Barge.BLUE_HIGHER.getY()) + : (y > Barge.RED_LOWER.getY()); } return false; } From 34dd4dc739f92b0dba5314b782d34429acd00035 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Wed, 19 Mar 2025 19:44:15 -0400 Subject: [PATCH 20/36] stuff --- src/main/java/frc/robot/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7e63d98b..74552023 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -253,8 +253,6 @@ private void configureBindings() { // SmartDashboard.putData("Climber reset", new ResetClimber(climber)); // SmartDashboard.putData("Climber stop", new CloseClimber(climber)); } - SmartDashboard.putNumber("X", 0.0); - SmartDashboard.putNumber("Y", 0.0); new Trigger(() -> Robot.getMode() == RobotMode.TELEOP) .onTrue(new RumbleController(drivetrain::getPose, controller)); } From 39516a7c12b11d6afdfde748ea6e89923cba47ab Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 20 Mar 2025 18:39:27 -0400 Subject: [PATCH 21/36] added toggle for rumble --- src/main/java/frc/robot/commands/RumbleController.java | 7 +++++-- src/main/java/frc/robot/constants/GameConstants.java | 1 + 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index bb654de1..feb0f363 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot; +import frc.robot.constants.Constants; import frc.robot.utils.Barge; import frc.robot.utils.logging.commands.LoggableCommand; import java.util.Optional; @@ -41,9 +42,11 @@ public void initialize() {} @Override public void execute() { - if (isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())) + if (isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())){ + if(Constants.RUMBLE_CONTROLLER){ controller.setRumble(RumbleType.kBothRumble, 0.5); - else controller.setRumble(RumbleType.kBothRumble, 0); + } + } else controller.setRumble(RumbleType.kBothRumble, 0); } @Override diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 36ea90e0..92d351f9 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -192,6 +192,7 @@ public enum Mode { public static final boolean HI_HI_SIMULATE_GRAVITY = true; 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; // ELEVATOR CONSTANTS public static final double ELEVATOR_MANUAL_DEADBAND = 0.2; From d45b9f5471dc00e6172cea2e605b87ca4dcf1080 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 20 Mar 2025 19:11:48 -0400 Subject: [PATCH 22/36] updated With noah changes --- src/main/java/frc/robot/commands/RumbleController.java | 6 +++--- src/main/java/frc/robot/utils/Barge.java | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index feb0f363..867385c9 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -29,10 +29,10 @@ public RumbleController(Supplier robotPose, CommandXboxController contro public static boolean isInOppositeBarge(double x, double y) { Optional al = Robot.getAllianceColor(); - if ((x < Barge.BLUE_HIGHER.getX()) && (x > Barge.RED_LOWER.getX()) && (al.isPresent())) { + if ((x < Barge.BLUER_HIGHER.getX()) && (x > Barge.BLUEL_LOWER.getX()) && (al.isPresent())) { return al.get().equals(Alliance.Red) - ? (y < Barge.BLUE_HIGHER.getY()) - : (y > Barge.RED_LOWER.getY()); + ? (y < Barge.BLUER_HIGHER.getY()) + : (y > Barge.BLUEL_LOWER.getY()+4); } return false; } diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index 2f43f716..ad8da94c 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -6,10 +6,10 @@ /** Add your docs here. */ public enum Barge { - BLUE_LOWER(8.9246, 0), - BLUE_HIGHER(8.9246, 4), - RED_LOWER(7.56, 4), - RED_HIGHER(7.56, 8); + BLUER_LOWER(8.9246, 0), + BLUER_HIGHER(8.9246, 4), + BLUEL_LOWER(7.56, 0), + BLUEL_HIGHER(7.56, 4); private final double x; private final double y; From 434b21f2b3cb7eb04fc24aedbbbdc965e281a97c Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 20 Mar 2025 19:13:06 -0400 Subject: [PATCH 23/36] name --- src/main/java/frc/robot/utils/Barge.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index ad8da94c..25bb0259 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -6,10 +6,10 @@ /** Add your docs here. */ public enum Barge { - BLUER_LOWER(8.9246, 0), - BLUER_HIGHER(8.9246, 4), - BLUEL_LOWER(7.56, 0), - BLUEL_HIGHER(7.56, 4); + BLUE_RIGHT_LOWER(8.9246, 0), + BLUE_RIGHT_HIGHER(8.9246, 4), + BLUE_LEFT_LOWER(7.56, 0), + BLUEL_LEFT_HIGHER(7.56, 4); private final double x; private final double y; From bea98367238d61fb9d8407c3d85e76337a810912 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 20 Mar 2025 19:25:15 -0400 Subject: [PATCH 24/36] add enum --- src/main/java/frc/robot/commands/RumbleController.java | 6 +++--- src/main/java/frc/robot/utils/Barge.java | 6 ++++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 867385c9..b2f4222a 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -29,10 +29,10 @@ public RumbleController(Supplier robotPose, CommandXboxController contro public static boolean isInOppositeBarge(double x, double y) { Optional al = Robot.getAllianceColor(); - if ((x < Barge.BLUER_HIGHER.getX()) && (x > Barge.BLUEL_LOWER.getX()) && (al.isPresent())) { + if ((x < Barge.BLUE_RIGHT_HIGHER.getX()) && (x > Barge.BLUE_LEFT_LOWER.getX()) && (al.isPresent())) { return al.get().equals(Alliance.Red) - ? (y < Barge.BLUER_HIGHER.getY()) - : (y > Barge.BLUEL_LOWER.getY()+4); + ? (y < Barge.BLUE_RIGHT_HIGHER.getY()) + : (y > Barge.BLUE_RIGHT_HIGHER.getY()); } return false; } diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index 25bb0259..ac53276b 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -4,6 +4,9 @@ package frc.robot.utils; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + /** Add your docs here. */ public enum Barge { BLUE_RIGHT_LOWER(8.9246, 0), @@ -26,4 +29,7 @@ public double getY() { public double getX() { return x; } + public double getXBasedOnAlliance(DriverStation.Alliance AllianceColor) { + return AllianceColor.equals(Alliance.Blue) ? x : x+4; + } } From 57790b769b393de2afc8bbd4497b541d71f8598a Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 20 Mar 2025 19:25:40 -0400 Subject: [PATCH 25/36] added max rumble --- src/main/java/frc/robot/commands/RumbleController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index b2f4222a..5dfd5392 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -44,7 +44,7 @@ public void initialize() {} public void execute() { if (isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())){ if(Constants.RUMBLE_CONTROLLER){ - controller.setRumble(RumbleType.kBothRumble, 0.5); + controller.setRumble(RumbleType.kBothRumble, 1); } } else controller.setRumble(RumbleType.kBothRumble, 0); } From a0ea03a37530ba60cbb5b4efbc806e849571b076 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 20 Mar 2025 19:28:03 -0400 Subject: [PATCH 26/36] added changes --- .../frc/robot/commands/RumbleController.java | 16 +++++++++------- src/main/java/frc/robot/utils/Barge.java | 9 +++------ 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 5dfd5392..565bb8fd 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -29,10 +29,12 @@ public RumbleController(Supplier robotPose, CommandXboxController contro public static boolean isInOppositeBarge(double x, double y) { Optional al = Robot.getAllianceColor(); - if ((x < Barge.BLUE_RIGHT_HIGHER.getX()) && (x > Barge.BLUE_LEFT_LOWER.getX()) && (al.isPresent())) { + if ((x < Barge.BLUE_RIGHT_HIGHER.getX()) + && (x > Barge.BLUE_LEFT_LOWER.getX()) + && (al.isPresent())) { return al.get().equals(Alliance.Red) - ? (y < Barge.BLUE_RIGHT_HIGHER.getY()) - : (y > Barge.BLUE_RIGHT_HIGHER.getY()); + ? (y < Barge.BLUE_RIGHT_HIGHER.getY(al.get())) + : (y > Barge.BLUE_RIGHT_HIGHER.getY(al.get())); } return false; } @@ -42,10 +44,10 @@ public void initialize() {} @Override public void execute() { - if (isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())){ - if(Constants.RUMBLE_CONTROLLER){ - controller.setRumble(RumbleType.kBothRumble, 1); - } + if (isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())) { + if (Constants.RUMBLE_CONTROLLER) { + controller.setRumble(RumbleType.kBothRumble, 1); + } } else controller.setRumble(RumbleType.kBothRumble, 0); } diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index ac53276b..b9b636b5 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -22,14 +22,11 @@ public enum Barge { this.y = y; } - public double getY() { - return y; - } - public double getX() { return x; } - public double getXBasedOnAlliance(DriverStation.Alliance AllianceColor) { - return AllianceColor.equals(Alliance.Blue) ? x : x+4; + + public double getY(DriverStation.Alliance AllianceColor) { + return AllianceColor.equals(Alliance.Blue) ? y : y + 4; } } From 6a5381bde279f96ed2e6953dcd8058038dfabbda Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Thu, 20 Mar 2025 19:31:41 -0400 Subject: [PATCH 27/36] names --- src/main/java/frc/robot/commands/RumbleController.java | 8 ++++---- src/main/java/frc/robot/utils/Barge.java | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 565bb8fd..65f0a089 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -29,12 +29,12 @@ public RumbleController(Supplier robotPose, CommandXboxController contro public static boolean isInOppositeBarge(double x, double y) { Optional al = Robot.getAllianceColor(); - if ((x < Barge.BLUE_RIGHT_HIGHER.getX()) - && (x > Barge.BLUE_LEFT_LOWER.getX()) + if ((x < Barge.RIGHT_HIGHER.getX()) + && (x > Barge.LEFT_LOWER.getX()) && (al.isPresent())) { return al.get().equals(Alliance.Red) - ? (y < Barge.BLUE_RIGHT_HIGHER.getY(al.get())) - : (y > Barge.BLUE_RIGHT_HIGHER.getY(al.get())); + ? (y < Barge.RIGHT_HIGHER.getY(al.get())) + : (y > Barge.RIGHT_HIGHER.getY(al.get())); } return false; } diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index b9b636b5..5979d905 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -9,10 +9,10 @@ /** Add your docs here. */ public enum Barge { - BLUE_RIGHT_LOWER(8.9246, 0), - BLUE_RIGHT_HIGHER(8.9246, 4), - BLUE_LEFT_LOWER(7.56, 0), - BLUEL_LEFT_HIGHER(7.56, 4); + 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; From 456a951568407a1255afd3a52a619b9da36346c1 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Fri, 21 Mar 2025 18:37:23 -0400 Subject: [PATCH 28/36] Updated#1 --- src/main/java/frc/robot/commands/RumbleController.java | 4 ++-- src/main/java/frc/robot/utils/Barge.java | 4 ++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 65f0a089..2e0aab89 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -33,8 +33,8 @@ public static boolean isInOppositeBarge(double x, double y) { && (x > Barge.LEFT_LOWER.getX()) && (al.isPresent())) { return al.get().equals(Alliance.Red) - ? (y < Barge.RIGHT_HIGHER.getY(al.get())) - : (y > Barge.RIGHT_HIGHER.getY(al.get())); + ? (y < Barge.RIGHT_HIGHER.getYFromAlliaceColor()) + : (y > Barge.RIGHT_HIGHER.getYFromAlliaceColor()); } return false; } diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index 5979d905..c46830a5 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Robot; /** Add your docs here. */ public enum Barge { @@ -29,4 +30,7 @@ public double getX() { public double getY(DriverStation.Alliance AllianceColor) { return AllianceColor.equals(Alliance.Blue) ? y : y + 4; } + public double getYFromAlliaceColor() { + return Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4; + } } From d48d4356309dd665a8b0f902537efb511520ead4 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Fri, 21 Mar 2025 18:42:11 -0400 Subject: [PATCH 29/36] added fieldZoneUtils --- .../frc/robot/commands/RumbleController.java | 20 ++----------- .../java/frc/robot/utils/FieldZoneUtils.java | 28 +++++++++++++++++++ 2 files changed, 30 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc/robot/utils/FieldZoneUtils.java diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 2e0aab89..854cec19 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -5,15 +5,11 @@ package frc.robot.commands; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Robot; import frc.robot.constants.Constants; -import frc.robot.utils.Barge; +import frc.robot.utils.FieldZoneUtils; import frc.robot.utils.logging.commands.LoggableCommand; -import java.util.Optional; 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 */ @@ -27,24 +23,12 @@ public RumbleController(Supplier robotPose, CommandXboxController contro this.controller = controller; } - 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.getYFromAlliaceColor()) - : (y > Barge.RIGHT_HIGHER.getYFromAlliaceColor()); - } - return false; - } - @Override public void initialize() {} @Override public void execute() { - if (isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())) { + if (FieldZoneUtils.isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())) { if (Constants.RUMBLE_CONTROLLER) { controller.setRumble(RumbleType.kBothRumble, 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..ded6ed6b --- /dev/null +++ b/src/main/java/frc/robot/utils/FieldZoneUtils.java @@ -0,0 +1,28 @@ +// 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 java.util.Optional; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Robot; + +/** 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.getYFromAlliaceColor()) + : (y > Barge.RIGHT_HIGHER.getYFromAlliaceColor()); + } + return false; + } + +} From 19b62bca7dbe31a62af0d347bb12ac9aadf5054e Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Mar 2025 14:37:34 -0400 Subject: [PATCH 30/36] added erro message / backup if we do not have an alliance color --- src/main/java/frc/robot/utils/Barge.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index c46830a5..e7c2e0d8 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -31,6 +31,11 @@ public double getY(DriverStation.Alliance AllianceColor) { return AllianceColor.equals(Alliance.Blue) ? y : y + 4; } public double getYFromAlliaceColor() { + if(Robot.getAllianceColor().isPresent()){ return Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4; + } + DriverStation.reportError( + "Alliance color is not present.", true); + return y; } } From 80ee41c0ee5c4b43f3152e0d55bfe6381ee2ec77 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Mar 2025 14:39:30 -0400 Subject: [PATCH 31/36] lint --- src/main/java/frc/robot/RobotContainer.java | 3 --- src/main/java/frc/robot/utils/Barge.java | 8 ++++---- src/main/java/frc/robot/utils/FieldZoneUtils.java | 14 +++++--------- 3 files changed, 9 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 261b0867..7f0de2bb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ 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; @@ -23,7 +22,6 @@ 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.coral.IntakeCoral; @@ -72,7 +70,6 @@ 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; diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index e7c2e0d8..739006e1 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -30,12 +30,12 @@ public double getX() { public double getY(DriverStation.Alliance AllianceColor) { return AllianceColor.equals(Alliance.Blue) ? y : y + 4; } + public double getYFromAlliaceColor() { - if(Robot.getAllianceColor().isPresent()){ - return Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4; + if (Robot.getAllianceColor().isPresent()) { + return Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4; } - DriverStation.reportError( - "Alliance color is not present.", true); + DriverStation.reportError("Alliance color is not present.", true); return y; } } diff --git a/src/main/java/frc/robot/utils/FieldZoneUtils.java b/src/main/java/frc/robot/utils/FieldZoneUtils.java index ded6ed6b..871f87e1 100644 --- a/src/main/java/frc/robot/utils/FieldZoneUtils.java +++ b/src/main/java/frc/robot/utils/FieldZoneUtils.java @@ -4,25 +4,21 @@ package frc.robot.utils; -import java.util.Optional; - 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) { + + 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())) { + if ((x < Barge.RIGHT_HIGHER.getX()) && (x > Barge.LEFT_LOWER.getX()) && (al.isPresent())) { return al.get().equals(Alliance.Red) ? (y < Barge.RIGHT_HIGHER.getYFromAlliaceColor()) : (y > Barge.RIGHT_HIGHER.getYFromAlliaceColor()); } return false; - } - + } } From 7c629c756d3d851afeb8948d58f919eeaf22d437 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sun, 23 Mar 2025 12:55:18 -0400 Subject: [PATCH 32/36] update --- src/main/java/frc/robot/utils/Barge.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index 739006e1..ee4c92a7 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -32,10 +32,8 @@ public double getY(DriverStation.Alliance AllianceColor) { } public double getYFromAlliaceColor() { - if (Robot.getAllianceColor().isPresent()) { - return Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4; - } - DriverStation.reportError("Alliance color is not present.", true); - return y; + return Robot.getAllianceColor().isPresent() + ? Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4 + : y; } } From 7b78d2e7cde571d8de76e0d4bb71cc9b7e585ba0 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sun, 23 Mar 2025 14:17:21 -0400 Subject: [PATCH 33/36] Actually used the command in robot container :cry: --- src/main/java/frc/robot/RobotContainer.java | 6 ++++ .../robot/commands/climber/StopClimber.java | 29 ++++++++++--------- .../robot/commands/sequences/CancelAll.java | 9 +++--- 3 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8444d61d..80f164bb 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.coral.IntakeCoral; @@ -74,6 +76,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; @@ -210,6 +213,9 @@ private void pathPlannerCommands() { } private void configureBindings() { + + 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/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), From 23c54b5f82de7562ee5449d804d4faa686d633a9 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Mon, 24 Mar 2025 08:58:54 -0400 Subject: [PATCH 34/36] Changed name of second GetY --- src/main/java/frc/robot/utils/Barge.java | 2 +- src/main/java/frc/robot/utils/FieldZoneUtils.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index ee4c92a7..5e0b1645 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -31,7 +31,7 @@ public double getY(DriverStation.Alliance AllianceColor) { return AllianceColor.equals(Alliance.Blue) ? y : y + 4; } - public double getYFromAlliaceColor() { + public double getY() { return Robot.getAllianceColor().isPresent() ? Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4 : y; diff --git a/src/main/java/frc/robot/utils/FieldZoneUtils.java b/src/main/java/frc/robot/utils/FieldZoneUtils.java index 871f87e1..788f3846 100644 --- a/src/main/java/frc/robot/utils/FieldZoneUtils.java +++ b/src/main/java/frc/robot/utils/FieldZoneUtils.java @@ -16,8 +16,8 @@ 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.getYFromAlliaceColor()) - : (y > Barge.RIGHT_HIGHER.getYFromAlliaceColor()); + ? (y < Barge.RIGHT_HIGHER.getY()) + : (y > Barge.RIGHT_HIGHER.getY()); } return false; } From 823c9c1060082703525fbcce6070b4089c94ae26 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Tue, 25 Mar 2025 18:22:19 -0400 Subject: [PATCH 35/36] added rumble toggle --- src/main/java/frc/robot/RobotContainer.java | 7 ++++--- src/main/java/frc/robot/commands/RumbleController.java | 2 -- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4bdf9621..4ff55f21 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -214,9 +214,10 @@ private void pathPlannerCommands() { } private void configureBindings() { - - new Trigger(() -> Robot.getMode() == RobotMode.TELEOP) - .onTrue(new RumbleController(drivetrain::getPose, controller)); + 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 index 854cec19..9587687a 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -29,9 +29,7 @@ public void initialize() {} @Override public void execute() { if (FieldZoneUtils.isInOppositeBarge(robotPose.get().getX(), robotPose.get().getY())) { - if (Constants.RUMBLE_CONTROLLER) { controller.setRumble(RumbleType.kBothRumble, 1); - } } else controller.setRumble(RumbleType.kBothRumble, 0); } From 025601f01b7cace33c2d9cdc3bdab2719b863585 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Tue, 25 Mar 2025 18:23:47 -0400 Subject: [PATCH 36/36] changed --- src/main/java/frc/robot/commands/RumbleController.java | 1 - src/main/java/frc/robot/utils/Barge.java | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/RumbleController.java b/src/main/java/frc/robot/commands/RumbleController.java index 9587687a..f693bbe9 100644 --- a/src/main/java/frc/robot/commands/RumbleController.java +++ b/src/main/java/frc/robot/commands/RumbleController.java @@ -7,7 +7,6 @@ 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.constants.Constants; import frc.robot.utils.FieldZoneUtils; import frc.robot.utils.logging.commands.LoggableCommand; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/utils/Barge.java b/src/main/java/frc/robot/utils/Barge.java index 5e0b1645..371eae6d 100644 --- a/src/main/java/frc/robot/utils/Barge.java +++ b/src/main/java/frc/robot/utils/Barge.java @@ -34,6 +34,6 @@ public double getY(DriverStation.Alliance AllianceColor) { public double getY() { return Robot.getAllianceColor().isPresent() ? Robot.getAllianceColor().get().equals(Alliance.Blue) ? y : y + 4 - : y; + : -1; } }