From 0554d6d6970bf9ee11e019ccf8f9ad152a68543f Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Thu, 2 Apr 2026 19:23:03 -0400 Subject: [PATCH] Updating the sim to have two cameras, and have everything be in Elastic --- src/main/deploy/elastic-layout.json | 166 ++++++++++++++++++++++++++++ src/main/java/frc/robot/Robot.java | 85 +++++++++++--- 2 files changed, 234 insertions(+), 17 deletions(-) create mode 100644 src/main/deploy/elastic-layout.json diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json new file mode 100644 index 0000000..96e2a9a --- /dev/null +++ b/src/main/deploy/elastic-layout.json @@ -0,0 +1,166 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [ + { + "title": "Robot Pose Modifier", + "x": 896.0, + "y": 512.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "xTranslation", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Robot Pose Modifier/xTranslation", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "yTranslation", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Robot Pose Modifier/yTranslation", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "zRotation", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Robot Pose Modifier/zRotation", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + ], + "containers": [ + { + "title": "Sim Field", + "x": 896.0, + "y": 0.0, + "width": 896.0, + "height": 512.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Sim Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "SimCamera1-processed", + "x": 0.0, + "y": 0.0, + "width": 896.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/SimCamera1-processed", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "SimCamera2-processed", + "x": 0.0, + "y": 384.0, + "width": 896.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/SimCamera2-processed", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Robot", + "x": 1152.0, + "y": 512.0, + "width": 512.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Sim Field/Robot", + "period": 0.06, + "data_type": "double[]", + "show_submit_button": true + } + }, + { + "title": "GHOST 1", + "x": 1152.0, + "y": 640.0, + "width": 512.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Sim Field/GHOST 1", + "period": 0.06, + "data_type": "double[]", + "show_submit_button": true + } + }, + { + "title": "GHOST 2", + "x": 1152.0, + "y": 768.0, + "width": 512.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Sim Field/GHOST 2", + "period": 0.06, + "data_type": "double[]", + "show_submit_button": true + } + } + ] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [] + } + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4632a22..2c9267c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,8 +26,10 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.net.WebServer; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -40,11 +42,16 @@ public class Robot extends TimedRobot { private VisionSystemSim visionSim; private SimCameraProperties cameraProperties; - private PhotonCamera camera; - private PhotonCameraSim cameraSim; - private PhotonPoseEstimator estimator; - private Transform3d realRobotToCamera; - private Transform3d testRobotToCamera; + private PhotonCamera camera1; + private PhotonCamera camera2; + private PhotonCameraSim cameraSim1; + private PhotonCameraSim cameraSim2; + private PhotonPoseEstimator estimator1; + private PhotonPoseEstimator estimator2; + private Transform3d realRobotToCamera1; + private Transform3d testRobotToCamera1; + private Transform3d realRobotToCamera2; + private Transform3d testRobotToCamera2; private double xTranslation; private double yTranslation; @@ -55,6 +62,8 @@ public class Robot extends TimedRobot { * initialization code. */ public Robot() { + WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); + visionSim = new VisionSystemSim("main"); visionSim.addAprilTags(AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded)); @@ -70,11 +79,15 @@ public class Robot extends TimedRobot { cameraProperties.setAvgLatencyMs(35); cameraProperties.setLatencyStdDevMs(5); - camera = new PhotonCamera("SimCamera"); - cameraSim = new PhotonCameraSim(camera, cameraProperties); - cameraSim.enableDrawWireframe(true); + camera1 = new PhotonCamera("SimCamera1"); + cameraSim1 = new PhotonCameraSim(camera1, cameraProperties); + cameraSim1.enableDrawWireframe(true); - realRobotToCamera = new Transform3d( + camera2 = new PhotonCamera("SimCamera2"); + cameraSim2 = new PhotonCameraSim(camera2, cameraProperties); + cameraSim2.enableDrawWireframe(true); + + realRobotToCamera1 = new Transform3d( Units.inchesToMeters(1.5), Units.inchesToMeters(-8.5), Units.inchesToMeters(28.5), @@ -85,7 +98,7 @@ public class Robot extends TimedRobot { ) ); - testRobotToCamera = new Transform3d( + testRobotToCamera1 = new Transform3d( Units.inchesToMeters(1.5), Units.inchesToMeters(-8.5), Units.inchesToMeters(28.5), @@ -96,12 +109,41 @@ public class Robot extends TimedRobot { ) ); - visionSim.addCamera(cameraSim, realRobotToCamera); + realRobotToCamera2 = new Transform3d( + Units.inchesToMeters(1.5), + Units.inchesToMeters(-10.5), + Units.inchesToMeters(28.5), + new Rotation3d( + Units.degreesToRadians(0), + Units.degreesToRadians(-24), + Units.degreesToRadians(-10) + ) + ); - estimator = new PhotonPoseEstimator( + testRobotToCamera2 = new Transform3d( + Units.inchesToMeters(1.5), + Units.inchesToMeters(-10.5), + Units.inchesToMeters(28.5), + new Rotation3d( + Units.degreesToRadians(0), + Units.degreesToRadians(-24), + Units.degreesToRadians(-10) + ) + ); + + visionSim.addCamera(cameraSim1, realRobotToCamera1); + visionSim.addCamera(cameraSim2, realRobotToCamera2); + + estimator1 = new PhotonPoseEstimator( AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded), PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - testRobotToCamera + testRobotToCamera1 + ); + + estimator2 = new PhotonPoseEstimator( + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded), + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + testRobotToCamera2 ); xTranslation = 510; @@ -142,13 +184,22 @@ public class Robot extends TimedRobot { visionSim.update(robotPose); SmartDashboard.putData("Sim Field", visionSim.getDebugField()); - List results = camera.getAllUnreadResults(); + List results1 = camera1.getAllUnreadResults(); + List results2 = camera2.getAllUnreadResults(); - if(results.size() != 0) { - Optional cameraPose = estimator.estimateCoprocMultiTagPose(results.get(results.size() - 1)); + if(results1.size() != 0) { + Optional cameraPose = estimator1.estimateCoprocMultiTagPose(results1.get(results1.size() - 1)); cameraPose.ifPresent((p) -> { - visionSim.getDebugField().getObject("GHOST").setPose(p.estimatedPose.toPose2d()); + visionSim.getDebugField().getObject("GHOST 1").setPose(p.estimatedPose.toPose2d()); + }); + } + + if(results2.size() != 0) { + Optional cameraPose = estimator2.estimateCoprocMultiTagPose(results2.get(results2.size() - 1)); + + cameraPose.ifPresent((p) -> { + visionSim.getDebugField().getObject("GHOST 2").setPose(p.estimatedPose.toPose2d()); }); } }