From 68d910a58c7fbf3591f51a60f50a452c389ee3db Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Wed, 25 Feb 2026 15:09:17 -0800 Subject: [PATCH] fixes --- .../robot/commands/gpm/Superstructure.java | 11 ++++++- .../frc/robot/constants/FieldConstants.java | 29 +++++++++++-------- .../java/frc/robot/util/PhaseManager.java | 5 +++- 3 files changed, 31 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 1ab9f93..e721a39 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -1,5 +1,7 @@ package frc.robot.commands.gpm; +import static edu.wpi.first.units.Units.Rotation; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; @@ -26,6 +28,7 @@ import frc.robot.subsystems.turret.Turret; import frc.robot.subsystems.turret.TurretConstants; import frc.robot.util.PhaseManager; import frc.robot.util.ShooterPhysics; +import frc.robot.util.PhaseManager.CurrentState; import frc.robot.util.ShooterPhysics.Constraints; import frc.robot.util.ShooterPhysics.TurretState; @@ -208,7 +211,13 @@ public class Superstructure extends Command { stowEverything(); } else { turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity); + + if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){ + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); + } else{ + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity); + } + shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); if (phaseManager.shouldFeed()) { diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 6fc6512..a9f60e7 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -20,8 +20,8 @@ public class FieldConstants { /** Height of the field [meters] */ public static final double FIELD_WIDTH = field.getFieldWidth(); - public static final double RED_BORDER = Units.inchesToMeters(180); - public static final double BLUE_BORDER = FIELD_LENGTH - Units.inchesToMeters(180); + public static final double RED_BORDER = FIELD_LENGTH/2 + Units.inchesToMeters(167.0); + public static final double BLUE_BORDER = FIELD_LENGTH/2 - Units.inchesToMeters(167.0); public static final double LEFT_SIDE_TARGET = FIELD_WIDTH * 0.25; public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.75; @@ -40,10 +40,10 @@ public class FieldConstants { /** Location of hub target */ public static final Translation3d HUB_BLUE = - new Translation3d(Units.inchesToMeters(156.8 + 20), FIELD_WIDTH/2, Units.inchesToMeters(72)); + new Translation3d(Units.inchesToMeters(182.11), FIELD_WIDTH/2, Units.inchesToMeters(72)); public static final Translation3d HUB_RED = - new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 - 20), FIELD_WIDTH/2, Units.inchesToMeters(72)); + new Translation3d(FIELD_LENGTH - Units.inchesToMeters(182.11), FIELD_WIDTH/2, Units.inchesToMeters(72)); public static final Translation3d NEUTRAL_LEFT = new Translation3d(FIELD_LENGTH/2, LEFT_SIDE_TARGET, 0); @@ -52,23 +52,23 @@ public class FieldConstants { new Translation3d(FIELD_LENGTH/2, RIGHT_SIDE_TARGET, 0); public static final Translation3d ALLIANCE_LEFT_BLUE = - new Translation3d(BLUE_BORDER - 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back + new Translation3d(BLUE_BORDER - 2, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back public static final Translation3d ALLIANCE_RIGHT_BLUE = - new Translation3d(BLUE_BORDER - 5, RIGHT_SIDE_TARGET, 0); + new Translation3d(BLUE_BORDER - 2, RIGHT_SIDE_TARGET, 0); public static final Translation3d ALLIANCE_LEFT_RED = - new Translation3d(RED_BORDER + 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back + new Translation3d(RED_BORDER + 2, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back public static final Translation3d ALLIANCE_RIGHT_RED = - new Translation3d(RED_BORDER + 5, RIGHT_SIDE_TARGET, 0); + new Translation3d(RED_BORDER + 2, RIGHT_SIDE_TARGET, 0); public static final Translation3d ALLIANCE_CENTER_BLUE = - new Translation3d(BLUE_BORDER - 5, FIELD_WIDTH/2, 0); + new Translation3d(BLUE_BORDER - 2, FIELD_WIDTH/2, 0); public static final Translation3d ALLIANCE_CENTER_RED = - new Translation3d(RED_BORDER + 5, FIELD_WIDTH/2, 0); + new Translation3d(RED_BORDER + 2, FIELD_WIDTH/2, 0); public static final double BLUE_ALLIANCE_LINE = BLUE_BORDER; // That's the distance from one side to the blue bump public static final double RED_ALLIANCE_LINE = RED_BORDER; // @@ -130,6 +130,10 @@ public class FieldConstants { public static FieldZone getZone(Translation2d drivepose) { double x = drivepose.getX(); //double y = drivepose.getY(); + if ((x < FIELD_LENGTH/2 - Units.inchesToMeters(120.0) && x > BLUE_ALLIANCE_LINE) + || x > FIELD_LENGTH/2 + Units.inchesToMeters(120.0) && x < RED_ALLIANCE_LINE){ + return FieldZone.TRENCH_BUMP; + } if(x < FieldConstants.RED_ALLIANCE_LINE) { // inside red if (Robot.getAlliance() == Alliance.Red) { return FieldZone.ALLIANCE; @@ -152,13 +156,14 @@ public class FieldConstants { if (Robot.getAlliance() == Alliance.Blue) { return ALLIANCE_LEFT_RED; } else { - return ALLIANCE_LEFT_BLUE; + // Reversed it so we shoot same side, but probably need to change this + return ALLIANCE_RIGHT_BLUE; } } else { if (Robot.getAlliance() == Alliance.Blue) { return ALLIANCE_RIGHT_RED; } else { - return ALLIANCE_RIGHT_BLUE; + return ALLIANCE_LEFT_BLUE; } } } diff --git a/src/main/java/frc/robot/util/PhaseManager.java b/src/main/java/frc/robot/util/PhaseManager.java index a62b290..61b1e75 100644 --- a/src/main/java/frc/robot/util/PhaseManager.java +++ b/src/main/java/frc/robot/util/PhaseManager.java @@ -19,6 +19,7 @@ public class PhaseManager { IDLE, STARTING_UP, TURNING_AROUND, + UNDER_TRENCH, SHOOTING, PASSING } @@ -51,6 +52,8 @@ public class PhaseManager { FieldZone zone = FieldConstants.getZone(drivePose.getTranslation()); if (zone == FieldConstants.FieldZone.ALLIANCE) { currentState = CurrentState.SHOOTING; + } else if (zone == FieldConstants.FieldZone.TRENCH_BUMP){ + currentState = CurrentState.UNDER_TRENCH; } else { currentState = CurrentState.PASSING; } @@ -78,7 +81,7 @@ public class PhaseManager { public boolean shouldFeed() { // TODO: I'm gonna comment out starting up until i find a solution - return !(currentState == CurrentState.TURNING_AROUND); //&& !(currentState == CurrentState.STARTING_UP); + return !(currentState == CurrentState.TURNING_AROUND) && !(currentState == CurrentState.UNDER_TRENCH); //&& !(currentState == CurrentState.STARTING_UP); } public Translation2d getTarget() { -- 2.39.5