From 66e685e8ccb2b26368da80d26291b027f35d475e Mon Sep 17 00:00:00 2001 From: murat <99989538+Murat65536@users.noreply.github.com> Date: Tue, 24 Feb 2026 19:19:43 -0500 Subject: [PATCH 1/2] Add bump slowdown --- src/main/java/frc/robot/DriverJoystick.java | 21 +++++++++++++--- .../frc/robot/constants/DriveConstants.java | 1 + .../frc/robot/constants/FieldConstants.java | 25 +++++++++++++++++++ 3 files changed, 44 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/constants/FieldConstants.java diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index fd50449..aefd455 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -2,9 +2,11 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.DriveConstants; +import frc.robot.constants.FieldConstants; import frc.robot.constants.IOConstants; import frc.robot.libraries.XboxController1038; import frc.robot.subsystems.DriveTrain; @@ -34,6 +36,9 @@ public class DriverJoystick extends XboxController1038 { private final Telemetry logger = new Telemetry(DriveConstants.MAX_SPEED); + private boolean toggleBumpSlowdown = true; + private double powerMultiplier = maxPower; + // Singleton Setup private static DriverJoystick instance; @@ -49,6 +54,16 @@ private DriverJoystick() { super(IOConstants.DRIVER_CONTROLLER_PORT); driveTrain.setDefaultCommand(this.driveTrain.applyRequest(() -> { + if (toggleBumpSlowdown) { + Translation2d translation = new Translation2d(driveTrain.getState().Pose.getX(), + driveTrain.getState().Pose.getY()); + if (FieldConstants.LEFT_BUMP.contains(translation) || FieldConstants.RIGHT_BUMP.contains(translation)) { + powerMultiplier = DriveConstants.BUMP_SLOWDOWN_POWER; + } else { + powerMultiplier = maxPower; + } + } + double sideways = this.getSidewaysValue(); double forward = this.getForwardValue(); double rotate = this.getRotateValue(); @@ -91,7 +106,7 @@ private DriverJoystick() { * @return sideways value */ private double getSidewaysValue() { - double x = this.getLeftX() * maxPower; + double x = this.getLeftX() * powerMultiplier; double sideways = limitRate(x, prevSideways, sidewaysLimiter); prevSideways = sideways; @@ -106,7 +121,7 @@ private double getSidewaysValue() { * @return forward value */ private double getForwardValue() { - double y = this.getLeftY() * maxPower; + double y = this.getLeftY() * powerMultiplier; double forward = limitRate(y, prevForward, forwardLimiter); prevForward = forward; @@ -121,7 +136,7 @@ private double getForwardValue() { * @return rotate value */ private double getRotateValue() { - double z = this.getRightX() * maxPower; + double z = this.getRightX() * powerMultiplier; double rotate = limitRate(z, prevRotate, rotateLimiter); prevRotate = rotate; diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 91eb032..e743fa1 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -15,6 +15,7 @@ public final class DriveConstants { public static final double DEFAULT_MAX_POWER = 0.75; public static final double OVERDRIVE_POWER = 1.0; + public static final double BUMP_SLOWDOWN_POWER = 0.3; // Chassis configuration // Distance between centers of right and left wheels on robot diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java new file mode 100644 index 0000000..506282f --- /dev/null +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -0,0 +1,25 @@ +package frc.robot.constants; + +import com.pathplanner.lib.util.FlippingUtil; + +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; + +public final class FieldConstants { + private FieldConstants() { + } + + private static final double HUB_EDGE_DISTANCE_FROM_DRIVER_STATION = Units.inchesToMeters(158.6); + private static final double BUMP_LENGTH = Units.inchesToMeters(44.4); + private static final double BUMP_WIDTH = Units.inchesToMeters(73); + private static final double TRENCH_WIDTH = Units.inchesToMeters(65.65); + private static final double LEFT_BUMP_DISTANCE = TRENCH_WIDTH; + private static final double RIGHT_BUMP_DISTANCE = FlippingUtil.fieldSizeY - TRENCH_WIDTH - BUMP_WIDTH; + public static final Rectangle2d LEFT_BUMP = new Rectangle2d( + new Translation2d(HUB_EDGE_DISTANCE_FROM_DRIVER_STATION, LEFT_BUMP_DISTANCE), + new Translation2d(HUB_EDGE_DISTANCE_FROM_DRIVER_STATION + BUMP_WIDTH, LEFT_BUMP_DISTANCE + BUMP_LENGTH)); + public static final Rectangle2d RIGHT_BUMP = new Rectangle2d( + new Translation2d(HUB_EDGE_DISTANCE_FROM_DRIVER_STATION, RIGHT_BUMP_DISTANCE), + new Translation2d(HUB_EDGE_DISTANCE_FROM_DRIVER_STATION + BUMP_WIDTH, RIGHT_BUMP_DISTANCE + BUMP_LENGTH)); +} From 219b9e877265b62be0966b4476670f2cec41b7b7 Mon Sep 17 00:00:00 2001 From: murat <99989538+Murat65536@users.noreply.github.com> Date: Tue, 24 Feb 2026 19:43:14 -0500 Subject: [PATCH 2/2] Have overdrive override bump slowdown --- src/main/java/frc/robot/DriverJoystick.java | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index aefd455..29ee137 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -36,9 +36,6 @@ public class DriverJoystick extends XboxController1038 { private final Telemetry logger = new Telemetry(DriveConstants.MAX_SPEED); - private boolean toggleBumpSlowdown = true; - private double powerMultiplier = maxPower; - // Singleton Setup private static DriverJoystick instance; @@ -54,13 +51,13 @@ private DriverJoystick() { super(IOConstants.DRIVER_CONTROLLER_PORT); driveTrain.setDefaultCommand(this.driveTrain.applyRequest(() -> { - if (toggleBumpSlowdown) { + if (maxPower != DriveConstants.OVERDRIVE_POWER) { Translation2d translation = new Translation2d(driveTrain.getState().Pose.getX(), driveTrain.getState().Pose.getY()); if (FieldConstants.LEFT_BUMP.contains(translation) || FieldConstants.RIGHT_BUMP.contains(translation)) { - powerMultiplier = DriveConstants.BUMP_SLOWDOWN_POWER; + maxPower = DriveConstants.BUMP_SLOWDOWN_POWER; } else { - powerMultiplier = maxPower; + maxPower = DriveConstants.DEFAULT_MAX_POWER; } } @@ -106,7 +103,7 @@ private DriverJoystick() { * @return sideways value */ private double getSidewaysValue() { - double x = this.getLeftX() * powerMultiplier; + double x = this.getLeftX() * maxPower; double sideways = limitRate(x, prevSideways, sidewaysLimiter); prevSideways = sideways; @@ -121,7 +118,7 @@ private double getSidewaysValue() { * @return forward value */ private double getForwardValue() { - double y = this.getLeftY() * powerMultiplier; + double y = this.getLeftY() * maxPower; double forward = limitRate(y, prevForward, forwardLimiter); prevForward = forward; @@ -136,7 +133,7 @@ private double getForwardValue() { * @return rotate value */ private double getRotateValue() { - double z = this.getRightX() * powerMultiplier; + double z = this.getRightX() * maxPower; double rotate = limitRate(z, prevRotate, rotateLimiter); prevRotate = rotate;