From 38480cb190a5320336451466000965a05872b638 Mon Sep 17 00:00:00 2001 From: Otto Date: Fri, 4 Oct 2024 22:47:31 -0400 Subject: [PATCH] Add field layout --- src/main/deploy/CrescendoFieldLayout.json | 152 +++++++++++ .../pathplanner/deploy/choreo/NewPath.traj | 247 ------------------ .../subsystems/AprilTagVision/Vision.java | 6 +- 3 files changed, 156 insertions(+), 249 deletions(-) create mode 100644 src/main/deploy/CrescendoFieldLayout.json delete mode 100644 src/main/deploy/pathplanner/deploy/choreo/NewPath.traj diff --git a/src/main/deploy/CrescendoFieldLayout.json b/src/main/deploy/CrescendoFieldLayout.json new file mode 100644 index 0000000..7cfb147 --- /dev/null +++ b/src/main/deploy/CrescendoFieldLayout.json @@ -0,0 +1,152 @@ +{ + "tags": [ + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } + } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/deploy/choreo/NewPath.traj b/src/main/deploy/pathplanner/deploy/choreo/NewPath.traj deleted file mode 100644 index 7360805..0000000 --- a/src/main/deploy/pathplanner/deploy/choreo/NewPath.traj +++ /dev/null @@ -1,247 +0,0 @@ -{ - "samples": [ - { - "x": 1.3, - "y": 5.65, - "heading": -1.6822477105769051e-31, - "angularVelocity": -1.1586339056536196e-33, - "velocityX": 0, - "velocityY": 3.306792997649108e-33, - "timestamp": 0 - }, - { - "x": 1.2914453481024677, - "y": 5.721603577140217, - "heading": -2.7664243623424044e-19, - "angularVelocity": -2.8771483148568663e-18, - "velocityX": -0.08897043356548494, - "velocityY": 0.7446943930987865, - "timestamp": 0.09615162649776889 - }, - { - "x": 1.2765167783316798, - "y": 5.865037698518956, - "heading": -8.89810241902797e-19, - "angularVelocity": -6.3770929967066984e-18, - "velocityX": -0.15526070972013933, - "velocityY": 1.4917492985110046, - "timestamp": 0.19230325299553777 - }, - { - "x": 1.2597297933990388, - "y": 6.080560649540843, - "heading": -1.9620741231567104e-18, - "angularVelocity": -1.115180239915778e-17, - "velocityX": -0.17458867357831137, - "velocityY": 2.2414904341414172, - "timestamp": 0.28845487949330667 - }, - { - "x": 1.2554319426090392, - "y": 6.367106638881104, - "heading": -7.790692176946905e-18, - "angularVelocity": -6.061902373721048e-17, - "velocityX": -0.04469868005925566, - "velocityY": 2.980147084115219, - "timestamp": 0.38460650599107554 - }, - { - "x": 1.3210994743610958, - "y": 6.63618584691843, - "heading": -1.2255698784768966e-17, - "angularVelocity": -4.643713885338712e-17, - "velocityX": 0.6829580959150883, - "velocityY": 2.7984883650779167, - "timestamp": 0.4807581324888444 - }, - { - "x": 1.4204739638729564, - "y": 6.841514817866321, - "heading": -1.203593420067165e-17, - "angularVelocity": 2.285606272376494e-18, - "velocityX": 1.0335185491029235, - "velocityY": 2.135470593964998, - "timestamp": 0.5769097589866133 - }, - { - "x": 1.5452179845383347, - "y": 6.979340897668785, - "heading": -1.2018281664509881e-17, - "angularVelocity": 1.835924233022629e-19, - "velocityX": 1.2973677639064556, - "velocityY": 1.4334243197192391, - "timestamp": 0.6730613854843822 - }, - { - "x": 1.692053973606, - "y": 7.048521518707275, - "heading": -1.2106843640419815e-17, - "angularVelocity": -9.210640161597751e-19, - "velocityX": 1.5271295392083102, - "velocityY": 0.7194950679289337, - "timestamp": 0.7692130119821511 - }, - { - "x": 1.8592442274093628, - "y": 7.048521518707275, - "heading": -1.2251441563868829e-17, - "angularVelocity": -1.5038513305774789e-18, - "velocityX": 1.7388187791835799, - "velocityY": -4.946586865198944e-32, - "timestamp": 0.86536463847992 - }, - { - "x": 1.9879333656829443, - "y": 7.064008956151006, - "heading": -1.3190680584565534e-17, - "angularVelocity": -1.556892948319027e-17, - "velocityX": 2.1331775022381367, - "velocityY": 0.25672293377238997, - "timestamp": 0.9256920787134001 - }, - { - "x": 2.1404131457459146, - "y": 7.094983825054881, - "heading": -1.3083373627998326e-17, - "angularVelocity": 1.7788308189151646e-18, - "velocityX": 2.5275360511398826, - "velocityY": 0.5134457683596676, - "timestamp": 0.9860195189468802 - }, - { - "x": 2.283844178238946, - "y": 7.120565712041231, - "heading": -1.0960209556709918e-17, - "angularVelocity": 3.5193883070410556e-17, - "velocityX": 2.3775421588882946, - "velocityY": 0.42405059600278056, - "timestamp": 1.0463469591803602 - }, - { - "x": 2.403484567934287, - "y": 7.130660169116252, - "heading": -8.80571994510131e-18, - "angularVelocity": 3.571314193460132e-17, - "velocityX": 1.9831835932727342, - "velocityY": 0.16732778708901472, - "timestamp": 1.1066743994138404 - }, - { - "x": 2.4993343047823786, - "y": 7.125267189594959, - "heading": -5.54249692656936e-18, - "angularVelocity": 5.409171591114853e-17, - "velocityX": 1.5888248610736313, - "velocityY": -0.08939513263645645, - "timestamp": 1.1670018396473205 - }, - { - "x": 2.571393385382689, - "y": 7.104386771326866, - "heading": -2.248096961272727e-18, - "angularVelocity": 5.4608511593593444e-17, - "velocityX": 1.194466072506608, - "velocityY": -0.34611808800880484, - "timestamp": 1.2273292798808006 - }, - { - "x": 2.619661808013916, - "y": 7.068018913269043, - "heading": 1.2436196481324329e-31, - "angularVelocity": 3.7264815441803186e-17, - "velocityX": 0.8001072554069414, - "velocityY": -0.6028410606689889, - "timestamp": 1.2876567201142808 - }, - { - "x": 2.632871044846576, - "y": 6.961183959736924, - "heading": 3.918184374353825e-18, - "angularVelocity": 3.8141920614884574e-17, - "velocityX": 0.12858636000692528, - "velocityY": -1.039993299386033, - "timestamp": 1.390383298829026 - }, - { - "x": 2.5767906511030385, - "y": 6.8099163657321276, - "heading": 6.6783092156810545e-18, - "angularVelocity": 2.6868680304119348e-17, - "velocityX": -0.5459190254842854, - "velocityY": -1.4725263500205321, - "timestamp": 1.493109877543771 - }, - { - "x": 2.4508660874710664, - "y": 6.6150930184391585, - "heading": 4.9673661737387426e-18, - "angularVelocity": -1.6655329487832808e-17, - "velocityX": -1.2258226177411307, - "velocityY": -1.8965232730485124, - "timestamp": 1.5958364562585161 - }, - { - "x": 2.2537930115679767, - "y": 6.378878861417135, - "heading": 4.64785469481977e-19, - "angularVelocity": -4.3830784019736315e-17, - "velocityX": -1.918423434017178, - "velocityY": -2.2994453818835865, - "timestamp": 1.6985630349732612 - }, - { - "x": 1.979196781802754, - "y": 6.114997756716588, - "heading": 5.213434951064863e-18, - "angularVelocity": 4.622610371128214e-17, - "velocityX": -2.673078702712734, - "velocityY": -2.568771470853776, - "timestamp": 1.8012896136880063 - }, - { - "x": 1.7075182270119558, - "y": 5.928998454213259, - "heading": 7.405818077675342e-18, - "angularVelocity": 2.134193440372557e-17, - "velocityX": -2.6446763650695857, - "velocityY": -1.8106249115832798, - "timestamp": 1.9040161924027514 - }, - { - "x": 1.5037591543500721, - "y": 5.789499175430237, - "heading": 6.275319228702638e-18, - "angularVelocity": -1.100493248589044e-17, - "velocityX": -1.9835087979322885, - "velocityY": -1.3579667553228336, - "timestamp": 2.0067427711174965 - }, - { - "x": 1.3679197271931591, - "y": 5.696499713659796, - "heading": 2.474180906252368e-18, - "angularVelocity": -3.700248106415516e-17, - "velocityX": -1.3223396403990193, - "velocityY": -0.9053106112732965, - "timestamp": 2.1094693498322417 - }, - { - "x": 1.3, - "y": 5.65, - "heading": -5.858520149608784e-33, - "angularVelocity": -2.4085111976372132e-17, - "velocityX": -0.6611699527328119, - "velocityY": -0.4526551379545377, - "timestamp": 2.2121959285469868 - }, - { - "x": 1.3, - "y": 5.65, - "heading": -8.249548083890788e-34, - "angularVelocity": 4.899894399656084e-32, - "velocityX": -1.7305340748157065e-31, - "velocityY": -3.342000801965607e-31, - "timestamp": 2.314922507261732 - } - ] -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/AprilTagVision/Vision.java b/src/main/java/frc/robot/subsystems/AprilTagVision/Vision.java index b5a7d9c..09841f4 100644 --- a/src/main/java/frc/robot/subsystems/AprilTagVision/Vision.java +++ b/src/main/java/frc/robot/subsystems/AprilTagVision/Vision.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.AprilTagVision; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -77,8 +78,9 @@ public Vision(Drive driveSubsystem) { // Cam layouts and stuff try { String deployDirectoryPath = Filesystem.getDeployDirectory().getAbsolutePath(); - // layout = new AprilTagFieldLayout(deployDirectoryPath + "/CrescendoFieldLayout.json"); - layout = AprilTagFieldLayout.k2024Crecendo.loadAprilTagLayoutField(); + layout = new AprilTagFieldLayout(deployDirectoryPath + "/CrescendoFieldLayout.json"); + // layout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + frontEstimator = new PhotonPoseEstimator( layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,