001/************************* PROJECT RON *************************/ 002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ 003/* Use of this source code is governed by an MIT-style license */ 004/* that can be found in the repository LICENSE file. */ 005/***************************************************************/ 006package com.stuypulse.robot.constants; 007 008import com.pathplanner.lib.util.FlippingUtil; 009import com.stuypulse.robot.Robot; 010import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; 011import com.stuypulse.robot.util.vision.AprilTag; 012import edu.wpi.first.math.geometry.Pose2d; 013import edu.wpi.first.math.geometry.Pose3d; 014import edu.wpi.first.math.geometry.Rotation2d; 015import edu.wpi.first.math.geometry.Rotation3d; 016import edu.wpi.first.math.geometry.Translation2d; 017import edu.wpi.first.math.geometry.Translation3d; 018import edu.wpi.first.math.util.Units; 019import edu.wpi.first.wpilibj.smartdashboard.Field2d; 020import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; 021import java.util.ArrayList; 022import java.util.List; 023 024/** This interface stores information about the field elements. */ 025public interface Field { 026 027 public static final Field2d FIELD2D = new Field2d(); 028 029 double WIDTH = Units.inchesToMeters(317.000); 030 031 double LENGTH = Units.inchesToMeters(650.000); 032 033 // Alliance relative hub center coordinates 034 public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); 035 036 public static final Pose3d hubCenter3d = new Pose3d(hubCenter.getX(), hubCenter.getY(), 037 Units.inchesToMeters(72), Rotation3d.kZero); 038 039 public static Pose2d getHubPose() { 040 return hubCenter; 041 } 042 043 // 1.0 meters from driverstation wall and field wall 044 public final Pose2d leftFerryZone = new Pose2d(1.0, WIDTH - 1.0, new Rotation2d()); 045 046 public final Pose2d rightFerryZone = new Pose2d(1.0, 1.0, new Rotation2d()); 047 048 public static Pose2d getFerryZonePose(Translation2d robot) { 049 if (robot.getDistance(leftFerryZone.getTranslation()) > robot 050 .getDistance(rightFerryZone.getTranslation())) { 051 return rightFerryZone; 052 } else { 053 return leftFerryZone; 054 } 055 } 056 057 // ZONE COORDINATES 058 // TODO: Temporary, need to get actual distance from outpost when intake pivot 059 // is deployed 060 public static final Pose2d outpost = new Pose2d(Units.inchesToMeters(25), Units.inchesToMeters(26.22), 061 Rotation2d.k180deg); 062 063 public static final Pose2d allianceZone = new Pose2d(Units.inchesToMeters(182.11), WIDTH, new Rotation2d()); 064 065 public static boolean inAllianceZone() { 066 return CommandSwerveDrivetrain.getInstance().getPose().getX() < allianceZone.getX(); 067 } 068 069 /** APRILTAGS ** */ 070 enum NamedTags { 071 072 // #1 073 RED_RIGHT_TRENCH_NZ, 074 RED_HUB_RIGHT_SIDE_MID, 075 RED_HUB_BACK_SIDE_LEFT, 076 RED_HUB_BACK_SIDE_MID, 077 // #5 078 RED_HUB_LEFT_SIDE_MID, 079 RED_LEFT_TRENCH_NZ, 080 RED_LEFT_TRENCH_AZ, 081 RED_HUB_LEFT_SIDE_RIGHT, 082 RED_HUB_FRONT_SIDE_LEFT, 083 // #10 084 RED_HUB_FRONT_SIDE_MID, 085 RED_HUB_RIGHT_SIDE_LEFT, 086 RED_RIGHT_TRENCH_AZ, 087 RED_HP_MID, 088 RED_HP_RIGHT, 089 // #15 090 RED_TOWER_MID, 091 RED_TOWER_RIGHT, 092 BLUE_RIGHT_TRENCH_NZ, 093 BLUE_HUB_RIGHT_SIDE_MID, 094 BLUE_HUB_BACK_SIDE_LEFT, 095 // #20 096 BLUE_HUB_BACK_SIDE_MID, 097 BLUE_HUB_LEFT_SIDE_MID, 098 BLUE_LEFT_TRENCH_NZ, 099 BLUE_LEFT_TRENCH_AZ, 100 BLUE_HUB_LEFT_SIDE_RIGHT, 101 // #25 102 BLUE_HUB_FRONT_SIDE_LEFT, 103 BLUE_HUB_FRONT_SIDE_MID, 104 BLUE_HUB_RIGHT_SIDE_LEFT, 105 BLUE_RIGHT_TRENCH_AZ, 106 BLUE_HP_MID, 107 // #30 108 BLUE_HP_RIGHT, 109 BLUE_TOWER_MID, 110 BLUE_TOWER_RIGHT; 111 112 public final AprilTag tag; 113 114 public int getID() { 115 return tag.id(); 116 } 117 118 public Pose3d getLocation() { 119 return Robot.isBlue() ? tag.location() : transformToOppositeAlliance(tag.location()); 120 } 121 122 private NamedTags() { 123 tag = APRILTAGS[ordinal()]; 124 } 125 } 126 127 AprilTag[] APRILTAGS = { // 2026 Field AprilTag Layout 128 new AprilTag( 129 1, 130 new Pose3d( 131 new Translation3d( 132 Units.inchesToMeters(467.64), 133 Units.inchesToMeters(292.31), 134 Units.inchesToMeters(35.00)), 135 new Rotation3d( 136 Units.degreesToRadians(0), 137 Units.degreesToRadians(0), 138 Units.degreesToRadians(180)))), 139 new AprilTag( 140 2, 141 new Pose3d( 142 new Translation3d( 143 Units.inchesToMeters(469.11), 144 Units.inchesToMeters(182.60), 145 Units.inchesToMeters(44.25)), 146 new Rotation3d( 147 Units.degreesToRadians(0), 148 Units.degreesToRadians(0), 149 Units.degreesToRadians(90)))), 150 new AprilTag( 151 3, 152 new Pose3d( 153 new Translation3d( 154 Units.inchesToMeters(445.35), 155 Units.inchesToMeters(172.84), 156 Units.inchesToMeters(44.25)), 157 new Rotation3d( 158 Units.degreesToRadians(0), 159 Units.degreesToRadians(0), 160 Units.degreesToRadians(180)))), 161 new AprilTag( 162 4, 163 new Pose3d( 164 new Translation3d( 165 Units.inchesToMeters(445.35), 166 Units.inchesToMeters(158.84), 167 Units.inchesToMeters(44.25)), 168 new Rotation3d( 169 Units.degreesToRadians(0), 170 Units.degreesToRadians(0), 171 Units.degreesToRadians(180)))), 172 new AprilTag( 173 5, 174 new Pose3d( 175 new Translation3d( 176 Units.inchesToMeters(469.11), 177 Units.inchesToMeters(135.09), 178 Units.inchesToMeters(44.25)), 179 new Rotation3d( 180 Units.degreesToRadians(0), 181 Units.degreesToRadians(0), 182 Units.degreesToRadians(270)))), 183 new AprilTag( 184 6, 185 new Pose3d( 186 new Translation3d( 187 Units.inchesToMeters(467.64), 188 Units.inchesToMeters(25.37), 189 Units.inchesToMeters(35.00)), 190 new Rotation3d( 191 Units.degreesToRadians(0), 192 Units.degreesToRadians(0), 193 Units.degreesToRadians(180)))), 194 new AprilTag( 195 7, 196 new Pose3d( 197 new Translation3d( 198 Units.inchesToMeters(470.59), 199 Units.inchesToMeters(25.37), 200 Units.inchesToMeters(35.00)), 201 new Rotation3d( 202 Units.degreesToRadians(0), 203 Units.degreesToRadians(0), 204 Units.degreesToRadians(0)))), 205 new AprilTag( 206 8, 207 new Pose3d( 208 new Translation3d( 209 Units.inchesToMeters(483.11), 210 Units.inchesToMeters(135.09), 211 Units.inchesToMeters(44.25)), 212 new Rotation3d( 213 Units.degreesToRadians(0), 214 Units.degreesToRadians(0), 215 Units.degreesToRadians(270)))), 216 new AprilTag( 217 9, 218 new Pose3d( 219 new Translation3d( 220 Units.inchesToMeters(492.88), 221 Units.inchesToMeters(144.84), 222 Units.inchesToMeters(44.25)), 223 new Rotation3d( 224 Units.degreesToRadians(0), 225 Units.degreesToRadians(0), 226 Units.degreesToRadians(0)))), 227 new AprilTag( 228 10, 229 new Pose3d( 230 new Translation3d( 231 Units.inchesToMeters(492.88), 232 Units.inchesToMeters(158.84), 233 Units.inchesToMeters(44.25)), 234 new Rotation3d( 235 Units.degreesToRadians(0), 236 Units.degreesToRadians(0), 237 Units.degreesToRadians(0)))), 238 new AprilTag( 239 11, 240 new Pose3d( 241 new Translation3d( 242 Units.inchesToMeters(483.11), 243 Units.inchesToMeters(182.60), 244 Units.inchesToMeters(44.25)), 245 new Rotation3d( 246 Units.degreesToRadians(0), 247 Units.degreesToRadians(0), 248 Units.degreesToRadians(90)))), 249 new AprilTag( 250 12, 251 new Pose3d( 252 new Translation3d( 253 Units.inchesToMeters(470.59), 254 Units.inchesToMeters(292.31), 255 Units.inchesToMeters(35.00)), 256 new Rotation3d( 257 Units.degreesToRadians(0), 258 Units.degreesToRadians(0), 259 Units.degreesToRadians(0)))), 260 new AprilTag( 261 13, 262 new Pose3d( 263 new Translation3d( 264 Units.inchesToMeters(650.92), 265 Units.inchesToMeters(291.47), 266 Units.inchesToMeters(21.75)), 267 new Rotation3d( 268 Units.degreesToRadians(0), 269 Units.degreesToRadians(0), 270 Units.degreesToRadians(180)))), 271 new AprilTag( 272 14, 273 new Pose3d( 274 new Translation3d( 275 Units.inchesToMeters(650.92), 276 Units.inchesToMeters(274.47), 277 Units.inchesToMeters(21.75)), 278 new Rotation3d( 279 Units.degreesToRadians(0), 280 Units.degreesToRadians(0), 281 Units.degreesToRadians(180)))), 282 new AprilTag( 283 15, 284 new Pose3d( 285 new Translation3d( 286 Units.inchesToMeters(650.90), 287 Units.inchesToMeters(170.22), 288 Units.inchesToMeters(21.75)), 289 new Rotation3d( 290 Units.degreesToRadians(0), 291 Units.degreesToRadians(0), 292 Units.degreesToRadians(180)))), 293 new AprilTag( 294 16, 295 new Pose3d( 296 new Translation3d( 297 Units.inchesToMeters(650.90), 298 Units.inchesToMeters(153.22), 299 Units.inchesToMeters(21.75)), 300 new Rotation3d( 301 Units.degreesToRadians(0), 302 Units.degreesToRadians(0), 303 Units.degreesToRadians(180)))), 304 new AprilTag( 305 17, 306 new Pose3d( 307 new Translation3d( 308 Units.inchesToMeters(183.59), 309 Units.inchesToMeters(25.37), 310 Units.inchesToMeters(35.00)), 311 new Rotation3d( 312 Units.degreesToRadians(0), 313 Units.degreesToRadians(0), 314 Units.degreesToRadians(0)))), 315 new AprilTag( 316 18, 317 new Pose3d( 318 new Translation3d( 319 Units.inchesToMeters(182.11), 320 Units.inchesToMeters(135.09), 321 Units.inchesToMeters(44.25)), 322 new Rotation3d( 323 Units.degreesToRadians(0), 324 Units.degreesToRadians(0), 325 Units.degreesToRadians(270)))), 326 new AprilTag( 327 19, 328 new Pose3d( 329 new Translation3d( 330 Units.inchesToMeters(205.87), 331 Units.inchesToMeters(144.84), 332 Units.inchesToMeters(44.25)), 333 new Rotation3d( 334 Units.degreesToRadians(0), 335 Units.degreesToRadians(0), 336 Units.degreesToRadians(0)))), 337 new AprilTag( 338 20, 339 new Pose3d( 340 new Translation3d( 341 Units.inchesToMeters(205.87), 342 Units.inchesToMeters(158.84), 343 Units.inchesToMeters(44.25)), 344 new Rotation3d( 345 Units.degreesToRadians(0), 346 Units.degreesToRadians(0), 347 Units.degreesToRadians(0)))), 348 new AprilTag( 349 21, 350 new Pose3d( 351 new Translation3d( 352 Units.inchesToMeters(182.11), 353 Units.inchesToMeters(182.60), 354 Units.inchesToMeters(44.25)), 355 new Rotation3d( 356 Units.degreesToRadians(0), 357 Units.degreesToRadians(0), 358 Units.degreesToRadians(90)))), 359 new AprilTag( 360 22, 361 new Pose3d( 362 new Translation3d( 363 Units.inchesToMeters(183.59), 364 Units.inchesToMeters(292.31), 365 Units.inchesToMeters(35.00)), 366 new Rotation3d( 367 Units.degreesToRadians(0), 368 Units.degreesToRadians(0), 369 Units.degreesToRadians(0)))), 370 new AprilTag( 371 23, 372 new Pose3d( 373 new Translation3d( 374 Units.inchesToMeters(180.64), 375 Units.inchesToMeters(292.31), 376 Units.inchesToMeters(35.00)), 377 new Rotation3d( 378 Units.degreesToRadians(0), 379 Units.degreesToRadians(0), 380 Units.degreesToRadians(180)))), 381 new AprilTag( 382 24, 383 new Pose3d( 384 new Translation3d( 385 Units.inchesToMeters(168.11), 386 Units.inchesToMeters(182.60), 387 Units.inchesToMeters(44.25)), 388 new Rotation3d( 389 Units.degreesToRadians(0), 390 Units.degreesToRadians(0), 391 Units.degreesToRadians(90)))), 392 new AprilTag( 393 25, 394 new Pose3d( 395 new Translation3d( 396 Units.inchesToMeters(158.34), 397 Units.inchesToMeters(172.84), 398 Units.inchesToMeters(44.25)), 399 new Rotation3d( 400 Units.degreesToRadians(0), 401 Units.degreesToRadians(0), 402 Units.degreesToRadians(180)))), 403 new AprilTag( 404 26, 405 new Pose3d( 406 new Translation3d( 407 Units.inchesToMeters(158.34), 408 Units.inchesToMeters(158.84), 409 Units.inchesToMeters(44.25)), 410 new Rotation3d( 411 Units.degreesToRadians(0), 412 Units.degreesToRadians(0), 413 Units.degreesToRadians(180)))), 414 new AprilTag( 415 27, 416 new Pose3d( 417 new Translation3d( 418 Units.inchesToMeters(168.11), 419 Units.inchesToMeters(135.09), 420 Units.inchesToMeters(44.25)), 421 new Rotation3d( 422 Units.degreesToRadians(0), 423 Units.degreesToRadians(0), 424 Units.degreesToRadians(270)))), 425 new AprilTag( 426 28, 427 new Pose3d( 428 new Translation3d( 429 Units.inchesToMeters(180.64), 430 Units.inchesToMeters(25.37), 431 Units.inchesToMeters(35.00)), 432 new Rotation3d( 433 Units.degreesToRadians(0), 434 Units.degreesToRadians(0), 435 Units.degreesToRadians(180)))), 436 new AprilTag( 437 29, 438 new Pose3d( 439 new Translation3d( 440 Units.inchesToMeters(0.30), 441 Units.inchesToMeters(26.22), 442 Units.inchesToMeters(21.75)), 443 new Rotation3d( 444 Units.degreesToRadians(0), 445 Units.degreesToRadians(0), 446 Units.degreesToRadians(0)))), 447 new AprilTag( 448 30, 449 new Pose3d( 450 new Translation3d( 451 Units.inchesToMeters(0.30), 452 Units.inchesToMeters(43.22), 453 Units.inchesToMeters(21.75)), 454 new Rotation3d( 455 Units.degreesToRadians(0), 456 Units.degreesToRadians(0), 457 Units.degreesToRadians(0)))), 458 new AprilTag( 459 31, 460 new Pose3d( 461 new Translation3d( 462 Units.inchesToMeters(0.32), 463 Units.inchesToMeters(147.47), 464 Units.inchesToMeters(21.75)), 465 new Rotation3d( 466 Units.degreesToRadians(0), 467 Units.degreesToRadians(0), 468 Units.degreesToRadians(0)))), 469 new AprilTag( 470 32, 471 new Pose3d( 472 new Translation3d( 473 Units.inchesToMeters(0.32), 474 Units.inchesToMeters(164.47), 475 Units.inchesToMeters(21.75)), 476 new Rotation3d( 477 Units.degreesToRadians(0), 478 Units.degreesToRadians(0), 479 Units.degreesToRadians(0)))) 480 }; 481 482 public static boolean isValidTag(int id) { 483 for (AprilTag tag : APRILTAGS) { 484 if (tag.id() == id) { 485 return true; 486 } 487 } 488 return false; 489 } 490 491 public static AprilTag getTag(int id) { 492 for (AprilTag tag : APRILTAGS) { 493 if (tag.id() == id) { 494 return tag; 495 } 496 } 497 return null; 498 } 499 500 public final int[] RED_HUB_TAG_IDS = { 2, 3, 4, 5, 8, 9, 10, 11 }; 501 502 public final int[] BLUE_HUB_TAG_IDS = { 18, 19, 20, 21, 24, 25, 26, 27 }; 503 504 public final int[] RED_TRENCH_TAG_IDS = { 1, 6, 7, 12 }; 505 506 public final int[] BLUE_TRENCH_TAG_IDS = { 17, 22, 23, 28 }; 507 508 public final int[] RED_TOWER_TAG_IDS = { 15, 16 }; 509 510 public final int[] BLUE_TOWER_TAG_IDS = { 31, 32 }; 511 512 public final int[] RED_HP_TAG_IDS = { 13, 14 }; 513 514 public final int[] BLUE_HP_TAG_IDS = { 29, 30 }; 515 516 public final int[] ALL_TAGS = { 517 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 518 27, 28, 29, 30, 31, 32 519 }; 520 521 public final Pose2d blueHubCenter = new Pose2d(Units.inchesToMeters(158.60), Units.inchesToMeters(158.84), 522 new Rotation2d()); 523 524 public static Pose2d getAllianceHubPose() { 525 return (Robot.isBlue() ? blueHubCenter : transformToOppositeAlliance(blueHubCenter)); 526 } 527 528 /* TRANSFORM FUNCTIONS */ 529 public static Pose3d transformToOppositeAlliance(Pose3d pose) { 530 Pose3d rotated = pose.rotateBy(new Rotation3d(0, 0, Math.PI)); 531 return new Pose3d( 532 rotated.getTranslation().plus(new Translation3d(LENGTH, WIDTH, 0)), 533 rotated.getRotation()); 534 } 535 536 public static Pose2d transformToOppositeAlliance(Pose2d pose) { 537 // Pose2d rotated = pose.rotateBy(Rotation2d.fromDegrees(180)); 538 // return new Pose2d( 539 // rotated.getTranslation().plus(new Translation2d(LENGTH, WIDTH)), 540 // rotated.getRotation()); 541 // return new Pose2d( 542 // rotated.getTranslation().plus(new Translation2d(LENGTH - pose.getX(), WIDTH - 543 // pose.getY())), 544 // rotated.getRotation() 545 // ); 546 return FlippingUtil.flipFieldPose(pose); 547 } 548 549 public static Translation2d transformToOppositeAlliance(Translation2d translation) { 550 return new Translation2d(LENGTH - translation.getX(), WIDTH - translation.getY()); 551 } 552 553 public static List<Pose2d> transformToOppositeAlliance(List<Pose2d> poses) { 554 List<Pose2d> newPoses = new ArrayList<>(); 555 for (Pose2d pose : poses) { 556 newPoses.add(transformToOppositeAlliance(pose)); 557 } 558 return newPoses; 559 } 560 561 /** * EMPTY FIELD POSES *** */ 562 Pose2d EMPTY_FIELD_POSE2D = new Pose2d(new Translation2d(-1, -1), new Rotation2d()); 563 564 Pose3d EMPTY_FIELD_POSE3D = new Pose3d(-1, -1, 0, new Rotation3d()); 565 566 public static void clearFieldObject(FieldObject2d fieldObject) { 567 fieldObject.setPose(EMPTY_FIELD_POSE2D); 568 } 569}