diff --git a/comp/build.gradle b/comp/build.gradle index 6a84da5..0756ee4 100644 --- a/comp/build.gradle +++ b/comp/build.gradle @@ -16,7 +16,7 @@ java { targetCompatibility = JavaVersion.VERSION_17 } -def ROBOT_MAIN_CLASS = "org.team100.frc2025.Main" +def ROBOT_MAIN_CLASS = "org.team100.frc2026.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/comp/simgui-ds.json b/comp/simgui-ds.json index 93a868b..fbf897e 100644 --- a/comp/simgui-ds.json +++ b/comp/simgui-ds.json @@ -4,11 +4,6 @@ "visible": false } }, - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/comp/src/main/deploy/2026-rebuilt-andymark.json b/comp/src/main/deploy/2026-rebuilt-andymark.json new file mode 100644 index 0000000..ecc0390 --- /dev/null +++ b/comp/src/main/deploy/2026-rebuilt-andymark.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.863959, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.863959, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9388636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9388636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.499332, + "y": 7.391907999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.499332, + "y": 6.960107999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6490636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6490636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.574159, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.574159, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0136906, + "y": 0.6507734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0136906, + "y": 1.0825734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} diff --git a/comp/src/main/deploy/2026-rebuilt-welded.json b/comp/src/main/deploy/2026-rebuilt-welded.json new file mode 100644 index 0000000..8c5a52d --- /dev/null +++ b/comp/src/main/deploy/2026-rebuilt-welded.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.8779798, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9154194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.3118646, + "y": 4.3902376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.3118646, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9154194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.8779798, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9528844, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2710194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.519177399999998, + "y": 3.6790375999999996, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.519177399999998, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2710194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9528844, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.5333172, + "y": 7.4033126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.5333172, + "y": 6.9715126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.5329616, + "y": 4.3235626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.5329616, + "y": 3.8917626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6630844, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6256194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.229174199999999, + "y": 3.6790375999999996, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.229174199999999, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6256194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6630844, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.5881798, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2700194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.0218614, + "y": 4.3902376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.0218614, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2700194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.5881798, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0077469999999999995, + "y": 0.6659626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0077469999999999995, + "y": 1.0977626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0080772, + "y": 3.7457125999999996, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0080772, + "y": 4.1775126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.069 + } +} diff --git a/comp/src/main/java/org/team100/frc2025/Main.java b/comp/src/main/java/org/team100/frc2026/Main.java similarity index 86% rename from comp/src/main/java/org/team100/frc2025/Main.java rename to comp/src/main/java/org/team100/frc2026/Main.java index e05ea04..2f0c4b1 100644 --- a/comp/src/main/java/org/team100/frc2025/Main.java +++ b/comp/src/main/java/org/team100/frc2026/Main.java @@ -1,4 +1,4 @@ -package org.team100.frc2025; +package org.team100.frc2026; import edu.wpi.first.wpilibj.RobotBase; diff --git a/comp/src/main/java/org/team100/frc2026/Robot.java b/comp/src/main/java/org/team100/frc2026/Robot.java new file mode 100644 index 0000000..61e5b97 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/Robot.java @@ -0,0 +1,146 @@ +package org.team100.frc2026; + +import org.team100.frc2026.robot.AllAutons; +import org.team100.frc2026.robot.Binder; +import org.team100.frc2026.robot.Machinery; +import org.team100.frc2026.robot.Prewarmer; +import org.team100.lib.coherence.Cache; +import org.team100.lib.coherence.Takt; +import org.team100.lib.config.Identity; +import org.team100.lib.experiments.Experiment; +import org.team100.lib.experiments.Experiments; +import org.team100.lib.framework.TimedRobot100; +import org.team100.lib.logging.RobotLog; +import org.team100.lib.util.Banner; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.WPILibVersion; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * This is the main robot class, which wires up events from TimedRobot100. + */ +public class Robot extends TimedRobot100 { + + private final RobotLog m_robotLog; + private final Machinery m_machinery; + private final AllAutons m_allAutons; + private final Binder m_binder; + + public Robot() { + Banner.printBanner(); + + // We want the CommandScheduler, not LiveWindow. + enableLiveWindowInTest(false); + + // This is for setting up LaserCAN devices. + // CanBridge.runTCP(); + + System.out.printf("WPILib Version: %s\n", WPILibVersion.Version); + System.out.printf("RoboRIO serial number: %s\n", RobotController.getSerialNumber()); + System.out.printf("Identity: %s\n", Identity.instance.name()); + RobotController.setBrownoutVoltage(5.5); + DriverStation.silenceJoystickConnectionWarning(true); + Experiments.instance.show(); + + // Log what the scheduler is doing. Use "withName()". + SmartDashboard.putData(CommandScheduler.getInstance()); + + m_robotLog = new RobotLog(); + + m_machinery = new Machinery(); + m_allAutons = new AllAutons(m_machinery); + m_binder = new Binder(m_machinery); + m_binder.bind(); + + Prewarmer.init(m_machinery); + } + + /** Called in the main loop. */ + @Override + public void robotPeriodic() { + // Advance the drumbeat. + Takt.update(); + // Take all the measurements we can, as soon and quickly as possible. + Cache.refresh(); + // Run one iteration of the command scheduler. + CommandScheduler.getInstance().run(); + m_machinery.periodic(); + m_robotLog.periodic(); + if (Experiments.instance.enabled(Experiment.FlushOften)) { + // StrUtil.warn("FLUSHING EVERY LOOP, DO NOT USE IN COMP"); + NetworkTableInstance.getDefault().flush(); + } + } + + ////////////////////////////////////////////////////////////////////// + // + // INITIALIZERS, DO NOT CHANGE THESE + // + + @Override + public void autonomousInit() { + Command auton = m_allAutons.get(); + if (auton == null) + return; + auton.schedule(); + } + + @Override + public void teleopInit() { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void close() { + super.close(); + m_machinery.close(); + m_allAutons.close(); + } + + /////////////////////////////////////////////////////////////////////// + // + // LEAVE ALL THESE EMPTY + // + + @Override + public void robotInit() { + } + + @Override + public void simulationInit() { + } + + @Override + public void disabledInit() { + } + + @Override + public void testInit() { + } + + @Override + public void simulationPeriodic() { + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopPeriodic() { + } + + @Override + public void testPeriodic() { + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/robot/AllAutons.java b/comp/src/main/java/org/team100/frc2026/robot/AllAutons.java new file mode 100644 index 0000000..5403ed3 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/robot/AllAutons.java @@ -0,0 +1,29 @@ +package org.team100.frc2026.robot; + +import org.team100.lib.config.AutonChooser; + +import edu.wpi.first.wpilibj2.command.Command; + +/** + * Populates the Auton chooser with all available autons. + * + * It's a good idea to instantiate them all here, even if you're not using them + * all, even if they're just in development, so they don't rot. + */ +public class AllAutons { + private final AutonChooser m_autonChooser; + + public AllAutons(Machinery machinery) { + m_autonChooser = new AutonChooser(); + + } + + public Command get() { + return m_autonChooser.get().command(); + } + + public void close() { + m_autonChooser.close(); + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/robot/Binder.java b/comp/src/main/java/org/team100/frc2026/robot/Binder.java new file mode 100644 index 0000000..8037577 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/robot/Binder.java @@ -0,0 +1,137 @@ +package org.team100.frc2026.robot; + +import java.util.function.BooleanSupplier; + +import org.team100.lib.controller.r1.FeedbackR1; +import org.team100.lib.controller.r1.PIDFeedback; +import org.team100.lib.hid.DriverXboxControl; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.Logging; +import org.team100.lib.subsystems.swerve.commands.SetRotation; +import org.team100.lib.subsystems.swerve.commands.manual.DriveManually; +import org.team100.lib.subsystems.swerve.commands.manual.ManualChassisSpeeds; +import org.team100.lib.subsystems.swerve.commands.manual.ManualFieldRelativeSpeeds; +import org.team100.lib.subsystems.swerve.commands.manual.ManualWithFullStateHeading; +import org.team100.lib.subsystems.swerve.commands.manual.ManualWithProfiledHeading; +import org.team100.lib.subsystems.swerve.commands.manual.ManualWithTargetLock; +import org.team100.lib.subsystems.swerve.commands.manual.SimpleManualModuleStates; +import org.team100.lib.subsystems.swerve.kinodynamics.limiter.SwerveLimiter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +/** + * Binds buttons to commands. Also creates default commands. + */ +public class Binder { + private static final LoggerFactory rootLogger = Logging.instance().rootLogger; + private static final LoggerFactory fieldLogger = Logging.instance().fieldLogger; + private final Machinery m_machinery; + + public Binder(Machinery machinery) { + m_machinery = machinery; + } + + public void bind() { + final LoggerFactory log = rootLogger.name("Commands"); + + ///////////////////////////////////////////////// + /// + /// CONTROLS + /// + final DriverXboxControl driver = new DriverXboxControl(0); + + ///////////////////////////////////////////////// + // + // DEFAULT COMMANDS + // + + SwerveLimiter limiter = new SwerveLimiter( + log, + m_machinery.m_swerveKinodynamics, + RobotController::getBatteryVoltage); + limiter.updateSetpoint(m_machinery.m_drive.getVelocity()); + + final DriveManually driveManually = new DriveManually( + driver::velocity, + m_machinery.m_localizer::setHeedRadiusM, + m_machinery.m_drive, + limiter); + m_machinery.m_drive.setDefaultCommand(driveManually.withName("drive default")); + final LoggerFactory manLog = log.type(driveManually); + + driveManually.register("MODULE_STATE", false, + new SimpleManualModuleStates(manLog, m_machinery.m_swerveKinodynamics)); + + driveManually.register("ROBOT_RELATIVE_CHASSIS_SPEED", false, + new ManualChassisSpeeds(manLog, m_machinery.m_swerveKinodynamics)); + + driveManually.register("FIELD_RELATIVE_TWIST", false, + new ManualFieldRelativeSpeeds(manLog, m_machinery.m_swerveKinodynamics)); + + final FeedbackR1 thetaFeedback = new PIDFeedback( + log, 3.2, 0, 0, true, 0.05, 1); + + driveManually.register("SNAPS_PROFILED", true, + new ManualWithProfiledHeading( + manLog, + m_machinery.m_swerveKinodynamics, + driver::pov, + thetaFeedback)); + + driveManually.register("SNAPS_FULL_STATE", true, + new ManualWithFullStateHeading( + manLog, + m_machinery.m_swerveKinodynamics, + driver::pov, + new double[] { + 5, + 0.35 + })); + + /** + * in reality, the target would come from some designator, e.g. buttons or + * camera or whatever. + */ + driveManually.register("LOCKED", false, + new ManualWithTargetLock( + fieldLogger, + manLog, + m_machinery.m_swerveKinodynamics, + () -> new Translation2d(6, 4), + thetaFeedback)); + /////////////////////////// + // + // DRIVETRAIN + // + + // Reset pose estimator so the current gyro rotation corresponds to zero. + onTrue(driver::back, + new SetRotation(m_machinery.m_drive, Rotation2d.kZero)); + + // Reset pose estimator so the current gyro rotation corresponds to 180. + onTrue(driver::start, + new SetRotation(m_machinery.m_drive, Rotation2d.kPi)); + + //////////////////////////////////////////////////////////// + // + // TEST + // + Tester tester = new Tester(m_machinery); + whileTrue(() -> (RobotState.isTest() && driver.a() && driver.b()), + tester.prematch()); + + } + + private static Trigger whileTrue(BooleanSupplier condition, Command command) { + return new Trigger(condition).whileTrue(command); + } + + private static Trigger onTrue(BooleanSupplier condition, Command command) { + return new Trigger(condition).onTrue(command); + } +} diff --git a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java new file mode 100644 index 0000000..5599b3d --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java @@ -0,0 +1,165 @@ +package org.team100.frc2026.robot; + +import java.io.IOException; + +import org.team100.lib.coherence.Takt; +import org.team100.lib.indicator.Beeper; +import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; +import org.team100.lib.localization.AprilTagRobotLocalizer; +import org.team100.lib.localization.NudgingVisionUpdater; +import org.team100.lib.localization.OdometryUpdater; +import org.team100.lib.localization.SimulatedTagDetector; +import org.team100.lib.localization.SwerveHistory; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.Logging; +import org.team100.lib.sensor.gyro.Gyro; +import org.team100.lib.sensor.gyro.GyroFactory; +import org.team100.lib.subsystems.swerve.SwerveDriveFactory; +import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; +import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; +import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; +import org.team100.lib.subsystems.swerve.module.SwerveModuleCollection; +import org.team100.lib.visualization.RobotPoseVisualization; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * This should contain all the hardware of the robot: all the subsystems etc + * that the Binder and Auton classes may want to use. + */ +public class Machinery { + // for background on drive current limits: + // https://v6.docs.ctr-electronics.com/en/stable/docs/hardware-reference/talonfx/improving-performance-with-current-limits.html + // https://www.chiefdelphi.com/t/the-brushless-era-needs-sensible-default-current-limits/461056/51 + // https://docs.google.com/document/d/10uXdmu62AFxyolmwtDY8_9UNnci7eVcev4Y64ZS0Aqk + // https://github.com/frc1678/C2024-Public/blob/17e78272e65a6ce4f87c00a3514c79f787439ca1/src/main/java/com/team1678/frc2024/Constants.java#L195 + // 2/26/25: Joel updated the supply limit to 90A, see 1678 code above. This is + // essentially unlimited, so you'll need to run some other kind of limiter (e.g. + // acceleration) to keep from browning out. + private static final double DRIVE_SUPPLY_LIMIT = 90; + private static final double DRIVE_STATOR_LIMIT = 110; + + private static final LoggerFactory logger = Logging.instance().rootLogger; + private static final LoggerFactory fieldLogger = Logging.instance().fieldLogger; + + private final Runnable m_robotViz; + private final SwerveModuleCollection m_modules; + private final Runnable m_simulatedTagDetector; + + final SwerveKinodynamics m_swerveKinodynamics; + final AprilTagRobotLocalizer m_localizer; + final SwerveDriveSubsystem m_drive; + final Beeper m_beeper; + + public Machinery() { + + final LoggerFactory driveLog = logger.name("Drive"); + m_swerveKinodynamics = SwerveKinodynamicsFactory.get(driveLog); + + //////////////////////////////////////////////////////////// + // + // SUBSYSTEMS + // + + // Subsystem initializers go here. + + //////////////////////////////////////////////////////////// + // + // VISUALIZATIONS + // + + // Visualization initializers go here + + //////////////////////////////////////////////////////////// + // + // POSE ESTIMATION + // + m_modules = SwerveModuleCollection.get( + driveLog, + DRIVE_SUPPLY_LIMIT, + DRIVE_STATOR_LIMIT, + m_swerveKinodynamics); + final Gyro gyro = GyroFactory.get( + driveLog, + m_swerveKinodynamics, + m_modules); + final SwerveHistory history = new SwerveHistory( + driveLog, + m_swerveKinodynamics, + gyro.getYawNWU(), + m_modules.positions(), + Pose2d.kZero, + Takt.get()); + final OdometryUpdater odometryUpdater = new OdometryUpdater( + m_swerveKinodynamics, gyro, history, m_modules::positions); + odometryUpdater.reset(Pose2d.kZero); + final NudgingVisionUpdater visionUpdater = new NudgingVisionUpdater( + history, odometryUpdater); + + //////////////////////////////////////////////////////////// + // + // CAMERA READERS + // + final AprilTagFieldLayoutWithCorrectOrientation layout = getLayout(); + + m_localizer = new AprilTagRobotLocalizer( + driveLog, + fieldLogger, + layout, + history, + visionUpdater); + + //////////////////////////////////////////////////////////// + // + // SIMULATED CAMERAS + // + m_simulatedTagDetector = SimulatedTagDetector.get(layout, history); + + //////////////////////////////////////////////////////////// + // + // DRIVETRAIN + // + m_drive = SwerveDriveFactory.get( + driveLog, + m_swerveKinodynamics, + m_localizer, + odometryUpdater, + history, + m_modules); + m_drive.resetPose(new Pose2d(m_drive.getPose().getTranslation(), new Rotation2d(Math.PI))); + m_robotViz = new RobotPoseVisualization( + fieldLogger, () -> m_drive.getState().pose(), "robot"); + + //////////////////////////////////////////////////////////// + // + // INDICATOR + // + + // There's no LED this year, unless we need it for testing or setup. + + // This makes beeps to warn about testing. + m_beeper = new Beeper(m_drive); + + } + + public void periodic() { + // publish the simulated tag sightings. + m_simulatedTagDetector.run(); + m_robotViz.run(); + } + + public void close() { + // this keeps the tests from conflicting via the use of simulated HAL ports. + m_modules.close(); + } + + /** Trap the IO exception. */ + private static AprilTagFieldLayoutWithCorrectOrientation getLayout() { + try { + return new AprilTagFieldLayoutWithCorrectOrientation(); + } catch (IOException e) { + throw new IllegalStateException("Could not read Apriltag layout file", e); + } + } +} diff --git a/comp/src/main/java/org/team100/frc2026/robot/Prewarmer.java b/comp/src/main/java/org/team100/frc2026/robot/Prewarmer.java new file mode 100644 index 0000000..6945ca2 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/robot/Prewarmer.java @@ -0,0 +1,7 @@ +package org.team100.frc2026.robot; + +public class Prewarmer { + public static void init(Machinery machinery) { + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/robot/README.md b/comp/src/main/java/org/team100/frc2026/robot/README.md new file mode 100644 index 0000000..2d718b5 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/robot/README.md @@ -0,0 +1,19 @@ +# robot + +The reason for the robot package is so that we can break up the large amount +of initialization we need to do into separate files, to reduce merge conflicts. + +The general idea is that all the mechanical stuff in the robot goes in +`Machinery`, all the trigger bindings go in `Binder`, and all the +autonomous behaviors go in `AllAutons`. + +From `Robot.java`: + +```java + m_machinery = new Machinery(); + m_allAutons = new AllAutons(m_machinery); + m_binder = new Binder(m_machinery); + m_binder.bind(); +``` + + diff --git a/comp/src/main/java/org/team100/frc2026/robot/Tester.java b/comp/src/main/java/org/team100/frc2026/robot/Tester.java new file mode 100644 index 0000000..22708a2 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/robot/Tester.java @@ -0,0 +1,71 @@ +package org.team100.frc2026.robot; + +import static edu.wpi.first.wpilibj2.command.Commands.parallel; +import static edu.wpi.first.wpilibj2.command.Commands.print; +import static edu.wpi.first.wpilibj2.command.Commands.runOnce; +import static edu.wpi.first.wpilibj2.command.Commands.sequence; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.Command; + +public class Tester { + + private static final String TEST = "TEST: "; + private final Machinery m_machinery; + private final Alert m_alert; + + public Tester(Machinery machinery) { + m_machinery = machinery; + m_alert = new Alert(text("waiting"), AlertType.kInfo); + m_alert.set(true); + } + + /** + * TEST ALL MOVEMENTS + * + * For pre- and post-match testing. + * + * Enable "test" mode and press driver "a" and "b" together. + * (in simulation this is buttons 1 and 2, or "z" and "x" on the keyboard) + * + * DANGER DANGER DANGER DANGER DANGER DANGER DANGER DANGER + * + * THIS WILL MOVE THE ROBOT VERY FAST! + * + * DO NOT RUN with the wheels on the floor! + * + * DO NOT RUN without tiedown clamps. + * + * DANGER DANGER DANGER DANGER DANGER DANGER DANGER DANGER + */ + public Command prematch() { + return sequence( + show("*** WARNING! MOTION STARTS IN 4 SECONDS ***"), + m_machinery.m_beeper.start(), + show("ahead slow"), + m_machinery.m_beeper.progress(), + m_machinery.m_drive.aheadSlow().withTimeout(1), + show("rightward slow"), + m_machinery.m_beeper.progress(), + m_machinery.m_drive.rightwardSlow().withTimeout(1), + show("done"), + m_machinery.m_beeper.done()) + .withName("test all movements"); + } + + /** Show the text as an alert and on the console. */ + private Command show(String text) { + return parallel( + runOnce(() -> { + m_alert.setText(text(text)); + m_alert.set(true); + }), + print(text(text))); + } + + private String text(String text) { + return TEST + text; + } + +} diff --git a/lib/src/main/deploy/2026-rebuilt-andymark.json b/lib/src/main/deploy/2026-rebuilt-andymark.json new file mode 100644 index 0000000..ecc0390 --- /dev/null +++ b/lib/src/main/deploy/2026-rebuilt-andymark.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.863959, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.863959, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9388636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9388636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.499332, + "y": 7.391907999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.499332, + "y": 6.960107999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6490636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6490636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.574159, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.574159, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0136906, + "y": 0.6507734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0136906, + "y": 1.0825734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} diff --git a/lib/src/main/deploy/2026-rebuilt-welded.json b/lib/src/main/deploy/2026-rebuilt-welded.json new file mode 100644 index 0000000..8c5a52d --- /dev/null +++ b/lib/src/main/deploy/2026-rebuilt-welded.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.8779798, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9154194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.3118646, + "y": 4.3902376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.3118646, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9154194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.8779798, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9528844, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2710194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.519177399999998, + "y": 3.6790375999999996, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.519177399999998, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2710194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9528844, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.5333172, + "y": 7.4033126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.5333172, + "y": 6.9715126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.5329616, + "y": 4.3235626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.5329616, + "y": 3.8917626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6630844, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6256194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.229174199999999, + "y": 3.6790375999999996, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.229174199999999, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6256194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6630844, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.5881798, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2700194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.0218614, + "y": 4.3902376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.0218614, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2700194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.5881798, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0077469999999999995, + "y": 0.6659626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0077469999999999995, + "y": 1.0977626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0080772, + "y": 3.7457125999999996, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0080772, + "y": 4.1775126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.069 + } +} diff --git a/lib/src/main/java/org/team100/lib/localization/AprilTagFieldLayoutWithCorrectOrientation.java b/lib/src/main/java/org/team100/lib/localization/AprilTagFieldLayoutWithCorrectOrientation.java index 088e2a9..903650d 100644 --- a/lib/src/main/java/org/team100/lib/localization/AprilTagFieldLayoutWithCorrectOrientation.java +++ b/lib/src/main/java/org/team100/lib/localization/AprilTagFieldLayoutWithCorrectOrientation.java @@ -39,11 +39,12 @@ * * @see https://github.com/AprilRobotics/apriltag/wiki/AprilTag-User-Guide#coordinate-system * - * NOTE: the AprilTag object is just the raw JSON, not corrected for alliance - * orientation. Do not use the AprilTag object! + * NOTE: the AprilTag object is just the raw JSON, not corrected for + * alliance + * orientation. Do not use the AprilTag object! */ public class AprilTagFieldLayoutWithCorrectOrientation { - private static final String FILENAME = "2025-reefscape.json"; + private static final String FILENAME = "2026-rebuilt-andymark.json"; // Inverts yaw private static final Transform3d FIX = new Transform3d( @@ -53,7 +54,12 @@ public class AprilTagFieldLayoutWithCorrectOrientation { private final Map layouts = new EnumMap<>(Alliance.class); public AprilTagFieldLayoutWithCorrectOrientation() throws IOException { - Path path = Filesystem.getDeployDirectory().toPath().resolve(FILENAME); + this(FILENAME); + } + + /** For testing only */ + AprilTagFieldLayoutWithCorrectOrientation(String filename) throws IOException { + Path path = Filesystem.getDeployDirectory().toPath().resolve(filename); AprilTagFieldLayout blueLayout = new AprilTagFieldLayout(path); blueLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); diff --git a/lib/src/test/java/org/team100/lib/localization/AprilTagFieldLayoutWithCorrectOrientationTest.java b/lib/src/test/java/org/team100/lib/localization/AprilTagFieldLayoutWithCorrectOrientationTest.java index f6900f6..8f1131c 100644 --- a/lib/src/test/java/org/team100/lib/localization/AprilTagFieldLayoutWithCorrectOrientationTest.java +++ b/lib/src/test/java/org/team100/lib/localization/AprilTagFieldLayoutWithCorrectOrientationTest.java @@ -15,7 +15,8 @@ public class AprilTagFieldLayoutWithCorrectOrientationTest { @Test void testGetTagPoseRed() throws IOException { - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation( + "2025-reefscape.json"); Pose3d pose = layout.getTagPose(Alliance.Red, 1).get(); // tag 1 coordinates for red assertEquals(0.851, pose.getX(), DELTA); @@ -25,7 +26,8 @@ void testGetTagPoseRed() throws IOException { @Test void testGetPoseBlue() throws IOException { - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation( + "2025-reefscape.json"); Pose3d pose = layout.getTagPose(Alliance.Blue, 1).get(); // tag 1 coordinates for blue assertEquals(16.697, pose.getX(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java index 98c4d9f..94f8530 100644 --- a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java +++ b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java @@ -37,7 +37,8 @@ class AprilTagRobotLocalizerTest implements Timeless { @Test void testEndToEnd() throws IOException, InterruptedException { - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation( + "2025-reefscape.json"); // these lists receive the updates final List poseEstimate = new ArrayList(); final List timeEstimate = new ArrayList(); @@ -107,7 +108,8 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { @Test void testEstimateRobotPose() throws IOException { - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation( + "2025-reefscape.json"); // these lists receive the updates final List poseEstimate = new ArrayList(); final List timeEstimate = new ArrayList(); @@ -161,7 +163,8 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { @Test void testEstimateRobotPose2() throws IOException { // robot is panned right 45, translation is ignored. - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation( + "2025-reefscape.json"); final List poseEstimate = new ArrayList(); final List timeEstimate = new ArrayList(); DoubleFunction history = t -> new ModelSE2(new Rotation2d(-Math.PI / 4)); @@ -239,7 +242,8 @@ void testCase1() throws IOException { // tag 4 in red is at about (0, 2.5) // tag 3 in red is at about (0, 3) - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation( + "2025-reefscape.json"); DoubleFunction history = t -> new ModelSE2(new Rotation2d(3 * Math.PI / 4)); @@ -280,7 +284,8 @@ void testCase2() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 2.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation( + "2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); @@ -318,7 +323,8 @@ void testCase2WithOffset() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 2.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); @@ -359,7 +365,8 @@ void testCase2WithTriangulation() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 2.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); @@ -402,7 +409,8 @@ void testCase2tilt() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 2.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); @@ -444,7 +452,8 @@ void testCase3() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 3.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); @@ -484,7 +493,8 @@ void testCase4() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 3.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); @@ -524,7 +534,8 @@ void testCase5() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 3.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); @@ -563,7 +574,8 @@ void testCase6() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 3.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); @@ -605,7 +617,8 @@ void testCase7() throws IOException { // -0.039, 2.662, 1.451 in red. // so the robot pose should be 1, 3.662, 1.451 - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tag4pose = layout.getTagPose(Alliance.Red, 4).get(); assertEquals(8.272, tag4pose.getX(), DELTA); assertEquals(1.914, tag4pose.getY(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/localization/PoseEstimationHelperTest.java b/lib/src/test/java/org/team100/lib/localization/PoseEstimationHelperTest.java index fc8c037..2ac5860 100644 --- a/lib/src/test/java/org/team100/lib/localization/PoseEstimationHelperTest.java +++ b/lib/src/test/java/org/team100/lib/localization/PoseEstimationHelperTest.java @@ -479,7 +479,8 @@ void testTagRotationCorrect24() throws IOException { // first try the "corrected" layout, which is "into the page" tag orientation. // this is CORRECT - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); Pose3d tagInFieldCoords = layout.getTagPose(Alliance.Blue, 7).get(); diff --git a/lib/src/test/java/org/team100/lib/localization/SimulatedTagDetectorTest.java b/lib/src/test/java/org/team100/lib/localization/SimulatedTagDetectorTest.java index 8eff61d..0c030a7 100644 --- a/lib/src/test/java/org/team100/lib/localization/SimulatedTagDetectorTest.java +++ b/lib/src/test/java/org/team100/lib/localization/SimulatedTagDetectorTest.java @@ -42,7 +42,8 @@ void testSimple() throws IOException { Camera.FUNNEL, Camera.CORAL_LEFT, Camera.CORAL_RIGHT); - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); // right in front of tag 7 SimulatedTagDetector sim = new SimulatedTagDetector( cameras, @@ -171,7 +172,8 @@ void testVisible1() { @Test void testTag6() throws IOException { Camera camera = Camera.TEST6; - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); // right in front of tag 7 Pose2d robotPose = new Pose2d(2.6576, 4.0259, Rotation2d.kZero); Pose3d robotPose3d = new Pose3d(robotPose); diff --git a/lib/src/test/java/org/team100/lib/localization/TagTest.java b/lib/src/test/java/org/team100/lib/localization/TagTest.java index 2b609ab..c663e19 100644 --- a/lib/src/test/java/org/team100/lib/localization/TagTest.java +++ b/lib/src/test/java/org/team100/lib/localization/TagTest.java @@ -10,8 +10,8 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Filesystem; /** * Remind myself what's in the JSON file. @@ -22,7 +22,8 @@ class TagTest { @Test void testBlueLayout() throws IOException { - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation( + "2025-reefscape.json"); /* * from the blue perspective, tag 7 has small x * and large y, and oriented at pi theta. @@ -39,7 +40,8 @@ void testBlueLayout() throws IOException { @Test void testRedLayout() throws IOException { - AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); + AprilTagFieldLayoutWithCorrectOrientation layout = + new AprilTagFieldLayoutWithCorrectOrientation("2025-reefscape.json"); /* * from the red perspective, tag 7 has large x