From 4fb6b2050d18b92cd3734c025d914c3ec2f24986 Mon Sep 17 00:00:00 2001 From: astrobot-coder Date: Sat, 28 Feb 2026 19:43:59 -0800 Subject: [PATCH 1/4] started field awareness --- .../java/org/team5924/frc2026/Constants.java | 4 ++- .../java/org/team5924/frc2026/RobotState.java | 3 ++ .../subsystems/awareness/FieldAwareness.java | 35 +++++++++++++++++++ .../frc2026/subsystems/drive/Drive.java | 3 +- 4 files changed, 43 insertions(+), 2 deletions(-) create mode 100644 src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 63dff412..981325d7 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -30,7 +30,7 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.util.Units; - +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.apriltag.AprilTagFieldLayout; @@ -78,6 +78,8 @@ public final class Field { public static final double CENTER_X = WIDTH / 2; public static final double CENTER_Y = LENGTH / 2; public static final double HUB_X = CENTER_X - 143.50; + + public static final Translation2d[] TRENCH_TRANSLATIONS = {new Translation2d(182.11, 25.7), new Translation2d(182.11, 292.718), new Translation2d(469.11, 25.7), new Translation2d(469.11, 292.718)}; } public final class GenericRollerSystem { diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 20a4e9ea..9a8aff1b 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import lombok.Getter; import lombok.Setter; import org.littletonrobotics.junction.AutoLogOutput; @@ -52,6 +53,8 @@ public static double getTime() { @Setter private Pose2d odometryPose = new Pose2d(); + @Getter @Setter private ChassisSpeeds robotChassisSpeeds = new ChassisSpeeds(); + @Getter @Setter @AutoLogOutput private Pose2d estimatedPose = Pose2d.kZero; public void resetPose(Pose2d pose) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java b/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java new file mode 100644 index 00000000..4ab53eff --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java @@ -0,0 +1,35 @@ +package org.team5924.frc2026.subsystems.awareness; + +import org.team5924.frc2026.Constants; +import org.team5924.frc2026.RobotState; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class FieldAwareness { + private static FieldAwareness instance; + + + public static FieldAwareness getInstance() { + if (instance == null) instance = new FieldAwareness(); + return instance; + } + + private double[] getDistanceToTrenches(){ + double[] distance = new double[4]; + Translation2d robotPosition = RobotState.getInstance().getOdometryPose().getTranslation(); + + for (int i = 0; i < 4; i++) { + distance[i] = Constants.Field.TRENCH_TRANSLATIONS[i].getDistance(robotPosition); + } + + return distance; + } + + private void tdfyguijoi() { // TOD: name later + ChassisSpeeds speeds = RobotState.getInstance().getRobotChassisSpeeds(); + Translation2d velocity = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + + + } +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/drive/Drive.java b/src/main/java/org/team5924/frc2026/subsystems/drive/Drive.java index 743bb44a..17a99597 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/drive/Drive.java +++ b/src/main/java/org/team5924/frc2026/subsystems/drive/Drive.java @@ -207,7 +207,7 @@ public Drive( // setpointGenerator = new SwerveSetpointGenerator(kinematics, // getModuleTranslations()); - // previousSetpoint = new SwerveSetpoint(getChassisSpeeds(), getModuleStates()); + // previousSetpoint = new SwerveSetpoint(getSpeeds(), getModuleStates()); SmartDashboard.putData( "Swerve Drive", @@ -305,6 +305,7 @@ public void periodic() { // Update RobotState RobotState.getInstance().setOdometryPose(getPose()); RobotState.getInstance().setEstimatedPose(getPose()); + RobotState.getInstance().setRobotChassisSpeeds(getChassisSpeeds()); // prevents error spam if (!gyroInputs.connected && wasGyroConnected) { From c34712ae130025ff5c120190581b669d2c9d3d43 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Mon, 2 Mar 2026 16:34:08 -0800 Subject: [PATCH 2/4] cleaner trench position contatns --- src/main/java/org/team5924/frc2026/Constants.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index e7b46ba7..fa17baa5 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -81,7 +81,15 @@ public final class Field { public static final double CENTER_Y = LENGTH / 2; public static final double HUB_X = CENTER_X - 143.50; - public static final Translation2d[] TRENCH_TRANSLATIONS = {new Translation2d(182.11, 25.7), new Translation2d(182.11, 292.718), new Translation2d(469.11, 25.7), new Translation2d(469.11, 292.718)}; + public static final double TRENCH_X_ALLIANCE = Units.inchesToMeters(182.11); + public static final double TRENCH_X_OPPONENT = Units.inchesToMeters(469.11); + public static final double TRENCH_Y_LEFT = Units.inchesToMeters(292.718); + public static final double TRENCH_Y_RIGHT = Units.inchesToMeters(25.7); + public static final Translation2d[] TRENCH_TRANSLATIONS = { + new Translation2d(TRENCH_X_ALLIANCE, TRENCH_Y_RIGHT), + new Translation2d(TRENCH_X_ALLIANCE, TRENCH_Y_LEFT), + new Translation2d(TRENCH_X_OPPONENT, TRENCH_Y_RIGHT), + new Translation2d(TRENCH_X_OPPONENT, TRENCH_Y_LEFT)}; } public final class GenericRollerSystem { From 7d0667ae80b3ee2a44636c5bc2f9909307472285 Mon Sep 17 00:00:00 2001 From: astrobot-coder Date: Tue, 3 Mar 2026 17:34:52 -0800 Subject: [PATCH 3/4] fixed the TODO typo --- .../team5924/frc2026/subsystems/awareness/FieldAwareness.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java b/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java index 4ab53eff..a59ba2da 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java +++ b/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java @@ -26,7 +26,7 @@ private double[] getDistanceToTrenches(){ return distance; } - private void tdfyguijoi() { // TOD: name later + private void tdfyguijoi() { // TODO: name later ChassisSpeeds speeds = RobotState.getInstance().getRobotChassisSpeeds(); Translation2d velocity = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); From 33ba566c614753f69628823bc7f2fd0bee302b9c Mon Sep 17 00:00:00 2001 From: astrobot-coder Date: Tue, 3 Mar 2026 20:11:19 -0800 Subject: [PATCH 4/4] made some more progress --- .../frc2026/subsystems/awareness/FieldAwareness.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java b/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java index a59ba2da..f34f20ea 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java +++ b/src/main/java/org/team5924/frc2026/subsystems/awareness/FieldAwareness.java @@ -15,21 +15,23 @@ public static FieldAwareness getInstance() { return instance; } - private double[] getDistanceToTrenches(){ - double[] distance = new double[4]; + private Translation2d[] getOffsetsToTrenches(){ + Translation2d[] offsets = new Translation2d[4]; Translation2d robotPosition = RobotState.getInstance().getOdometryPose().getTranslation(); for (int i = 0; i < 4; i++) { - distance[i] = Constants.Field.TRENCH_TRANSLATIONS[i].getDistance(robotPosition); + offsets[i] = Constants.Field.TRENCH_TRANSLATIONS[i].minus(robotPosition); } - return distance; + return offsets; } private void tdfyguijoi() { // TODO: name later ChassisSpeeds speeds = RobotState.getInstance().getRobotChassisSpeeds(); Translation2d velocity = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - + for (int i = 0; i < 4; i++) { + + } } }