diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index fd50449..29ee137 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; @@ -49,6 +51,16 @@ private DriverJoystick() { super(IOConstants.DRIVER_CONTROLLER_PORT); driveTrain.setDefaultCommand(this.driveTrain.applyRequest(() -> { + 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)) { + maxPower = DriveConstants.BUMP_SLOWDOWN_POWER; + } else { + maxPower = DriveConstants.DEFAULT_MAX_POWER; + } + } + double sideways = this.getSidewaysValue(); double forward = this.getForwardValue(); double rotate = this.getRotateValue(); 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)); +}