From 776e3ed9d34629773027fe3798b727feaa963592 Mon Sep 17 00:00:00 2001 From: Scott Jackson Date: Mon, 13 Apr 2026 18:30:45 -0400 Subject: [PATCH 1/2] Helper functions for regions and targets --- src/main/java/frc/robot/Constants.java | 46 +++++++++ src/main/java/frc/robot/Utils.java | 132 +++++++++++++++++++++++++ 2 files changed, 178 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fc73400..218b824 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,6 +9,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.units.measure.Angle; @@ -170,5 +171,50 @@ public final class Constants { public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + // These are the field targets for passing fuel to the Alliance Zone or to the Neutral Zone from the opponent's side. + // Left/Right are from driver's perspective. Coordinates assume origin at blue side of field. + // Rotation are zero because they don't matter. + public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d()); + public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d()); + public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d()); + public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d()); + + public static Pose3d RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 1.72, 0, new Rotation3d()); + public static Pose3d RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 6.35, 0, new Rotation3d()); + public static Pose3d RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d()); + public static Pose3d RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d()); + + + } + + // These represent the dividing lines for the regions in our shooting strategy. + public static class ShootingRegionDimensions { + + public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5; + public static double RED_ALLIANCE_ZONE_REGION_X_MIN = 13.0; + public static double NEUTRAL_ZONE_REGION_X_MIN = 5.5; + public static double NEUTRAL_ZONE_REGION_X_MAX = 11.0; + public static double BLUE_RIGHT_REGION_Y_MAX = 3.0; + public static double RED_LEFT_REGION_Y_MAX = 3.0; // same meaning as the previous one, but from red perspective + public static double BLUE_LEFT_REGION_Y_MIN = 5.0; + public static double RED_RIGHT_REGION_Y_MIN = 5.0; // same meaning as the previous one, but from red perspective + + } + + // Regions that define the AutoTurret shooting behaviors. + // Color ID in the name reflects robot's alliance since region definitions vary by alliance. + // Left/Right are from driver's perspective. + public enum ShootingRegion { + NON_SHOOTING_REGION, // Any part of the field that's not in one of the regions + BLUE_OWN_ALLIANCE_ZONE, + BLUE_LEFT_NEUTRAL_ZONE, + BLUE_RIGHT_NEUTRAL_ZONE, + BLUE_LEFT_OPPONENT_ALLIANCE_ZONE, + BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE, + RED_OWN_ALLIANCE_ZONE, + RED_LEFT_NEUTRAL_ZONE, + RED_RIGHT_NEUTRAL_ZONE, + RED_LEFT_OPPONENT_ALLIANCE_ZONE, + RED_RIGHT_OPPONENT_ALLIANCE_ZONE } } diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java index 64bd85f..9899985 100644 --- a/src/main/java/frc/robot/Utils.java +++ b/src/main/java/frc/robot/Utils.java @@ -1,5 +1,11 @@ package frc.robot; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Constants; +import frc.robot.Constants.ShootingRegion; +import frc.robot.Constants.ShootingRegionDimensions; +import edu.wpi.first.math.geometry.Pose3d; + public class Utils { /** @@ -22,4 +28,130 @@ public class Utils { return robotTurretRotations; } + + + /** + * Determines the region for the robot based on its location and alliance. + * Assumes the robot is a 27" square. Thresholds based on observations from simulator. + * @param robotPose: the robot's pose + * @param alliance: the red/blue alliance of the robot, as a sanity check + * @return the robot's shooting region + */ + public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){ + ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION; // the default return value if robot isn't in a valid region + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + + // Receommended: Add a sanity check for invalid robot poses. + + if (alliance == Alliance.Blue) { + if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) { + region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE; + } + else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN && + robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX && + robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) { + region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE; + } + else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN && + robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX && + robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) { + region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE; + } + else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN && + robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) { + region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE; + } + else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN && + robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) { + region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE; + } + } + else if (alliance == Alliance.Red) { + if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) { + region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE; + } + else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN && + robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX && + robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) { + region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE; + } + else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN && + robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX && + robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) { + region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE; + } + else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX && + robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) { + region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE; + } + else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN && + robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) { + region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE; + } + } + return region; + } + + + + /** + * Determines the appropriate shooting target for the robot based on its alliance + * and its current region on the field. There are 10 possible targets: + * Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone, + * and these exist for both red and blue alliances. + * Right/left are from driver's perspective. + * @param region: the identifier of the robot's current field region + * @param alliance: the red/blue alliance of the robot, as a sanity check + * @return the shooting target for a robot with the provided properties. Returns all-zero + * Pose3d object if the region/alliance combination is invalid. + */ + public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){ + + Pose3d shootingTarget = new Pose3d(); + if (alliance == Alliance.Blue) { + switch (region) { + case BLUE_OWN_ALLIANCE_ZONE: + shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE; + break; + case BLUE_LEFT_NEUTRAL_ZONE: + shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE; + break; + case BLUE_RIGHT_NEUTRAL_ZONE: + shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE; + break; + case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE: + shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE; + break; + case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE: + shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE; + break; + default: + break; + } + } + else if (alliance == Alliance.Red) { + switch (region) { + case RED_OWN_ALLIANCE_ZONE: + shootingTarget = Constants.FieldPoses.RED_HUB_POSE; + break; + case RED_LEFT_NEUTRAL_ZONE: + shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE; + break; + case RED_RIGHT_NEUTRAL_ZONE: + shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE; + break; + case RED_LEFT_OPPONENT_ALLIANCE_ZONE: + shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE; + break; + case RED_RIGHT_OPPONENT_ALLIANCE_ZONE: + shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE; + break; + default: + break; + } + } + return shootingTarget; + } + } From 7d21c401225a29df4b4dae8c78f1174839f31364 Mon Sep 17 00:00:00 2001 From: Team578 Driver Station Date: Mon, 13 Apr 2026 18:48:54 -0400 Subject: [PATCH 2/2] Added RunIntake command --- .../pathplanner/autos/1. Hub to Shoot.auto | 19 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 1 + 2 files changed, 20 insertions(+) diff --git a/src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto b/src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto index 619ff88..732cd97 100644 --- a/src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto +++ b/src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto @@ -35,6 +35,25 @@ "pathName": "1.2 Shoot to Depot Path" } }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + } + ] + } + }, { "type": "path", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6d68cd..f5e2972 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,6 +56,7 @@ public class RobotContainer { configureBindings(); NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake()); turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); }