From 024091a6c6d2ac74e3b120359022a337687d3f99 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 14 Feb 2026 14:04:11 -0800 Subject: [PATCH 1/3] Add new camera positions --- src/main/java/frc/robot/RobotContainer.java | 8 +++++--- .../frc/robot/subsystems/vision/VisionConstants.java | 12 +++++++++++- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1e16f7..b61a52b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -157,7 +157,9 @@ public RobotContainer() { vision_ = new AprilTagVision( drivebase_::addVisionMeasurement, - new CameraIOPhotonSim("front", VisionConstants.frontTransform, MapleSimUtil::getPosition, true) + new CameraIOPhotonSim("front", VisionConstants.frontTransform, MapleSimUtil::getPosition, true), + new CameraIOPhotonSim("backleft", VisionConstants.backLeftTransform, MapleSimUtil::getPosition, true), + new CameraIOPhotonSim("backright", VisionConstants.backRightTransform, MapleSimUtil::getPosition, true) ); intake_= new IntakeSubsystem(new IntakeIOSim()); @@ -240,11 +242,11 @@ public RobotContainer() { if (vision_ == null) { int numCams = switch (Constants.getRobot()) { - default -> 1; + default -> 3; }; CameraIO[] cams = new CameraIO[numCams]; - Arrays.fill(cams, new CameraIO() {}); + Arrays.setAll(cams, i -> new CameraIO() {}); vision_ = new AprilTagVision( drivebase_::addVisionMeasurement, diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 999a756..34515a6 100755 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -18,10 +18,20 @@ public class VisionConstants { // Transforms public static final Transform3d frontTransform = new Transform3d( - new Translation3d(Meters.of(0.1975), Meters.of(-0.2032), Meters.of(0.205)), + new Translation3d(Inches.of(0.125), Meters.of(-0.2032), Inches.of(20.7)), new Rotation3d(Degrees.zero(), Degrees.of(-20), Degrees.zero()) ); + public static final Transform3d backLeftTransform = new Transform3d( + new Translation3d(Meters.of(-0.155), Meters.of(0.318), Meters.of(0.213)), + new Rotation3d(Degrees.zero(), Degrees.of(-30), Degrees.of(150)) + ); + + public static final Transform3d backRightTransform = new Transform3d( + new Translation3d(Meters.of(-0.155), Meters.of(-0.318), Meters.of(0.213)), + new Rotation3d(Degrees.zero(), Degrees.of(-30), Degrees.of(210)) + ); + // Odometry Filtering Configuration public static final int minimumTagCount = 1; public static final double maximumAmbiguity = 0.3; // For Photonvision Sim From fff41d7f5f5bfdb9d0db3f8c68010b61294e7cbf Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 14 Feb 2026 14:21:16 -0800 Subject: [PATCH 2/3] Add new camera positions to simulation and model --- .advantagescope/Robot_Relic2026/config.json | 152 ++++++++++++------ .../subsystems/vision/VisionConstants.java | 4 +- 2 files changed, 101 insertions(+), 55 deletions(-) diff --git a/.advantagescope/Robot_Relic2026/config.json b/.advantagescope/Robot_Relic2026/config.json index e4ec428..47d03e4 100644 --- a/.advantagescope/Robot_Relic2026/config.json +++ b/.advantagescope/Robot_Relic2026/config.json @@ -1,54 +1,100 @@ { - "name": "Relic", - "rotations": [ - {"axis": "x", "degrees": 90}, - {"axis": "z", "degrees": 90} - ], - "position": [0, 0, 0], - "disableSimplification": true, - "dynamicColoring": true, - "cameras": [ - { - "name": "Front Camera", - "rotations": [ - { - "axis": "y", - "degrees": -20 - } - ], - "position": [ - 0.3048, - 0.12, - 0.12 - ], - "resolution": [ - 1280, - 960 - ], - "fov": 83 - } - ], - "components": [ - { - "zeroedRotations": [ - {"axis": "x", "degrees": 90}, - {"axis": "z", "degrees": 90} - ], - "zeroedPosition": [0.098, 0, -0.457] - }, - { - "zeroedRotations": [ - {"axis": "x", "degrees": 90}, - {"axis": "z", "degrees": 90} - ], - "zeroedPosition": [-0.252, 0, -0.207] - }, - { - "zeroedRotations": [ - {"axis": "x", "degrees": 90}, - {"axis": "z", "degrees": 90} - ], - "zeroedPosition": [-0.252, 0, -0.207] - } - ] - } + "name": "Relic", + "rotations": [ + {"axis": "x", "degrees": 90}, + {"axis": "z", "degrees": 90} + ], + "position": [0, 0, 0], + "disableSimplification": true, + "dynamicColoring": true, + "cameras": [ + { + "name": "Shooter Camera", + "rotations": [ + { + "axis": "y", + "degrees": -28 + } + ], + "position": [ + -0.03, + -0.003, + 0.526 + ], + "resolution": [ + 1280, + 960 + ], + "fov": 83 + }, + { + "name": "Back Left Camera", + "rotations": [ + { + "axis": "y", + "degrees": -30 + }, + { + "axis": "z", + "degrees": 150 + } + ], + "position": [ + -0.155, + 0.318, + 0.213 + ], + "resolution": [ + 1280, + 960 + ], + "fov": 83 + }, + { + "name": "Back Right Camera", + "rotations": [ + { + "axis": "y", + "degrees": -30 + }, + { + "axis": "z", + "degrees": 210 + } + ], + "position": [ + -0.155, + -0.318, + 0.213 + ], + "resolution": [ + 1280, + 960 + ], + "fov": 83 + } + ], + "components": [ + { + "zeroedRotations": [ + {"axis": "x", "degrees": 90}, + {"axis": "z", "degrees": 90} + ], + "zeroedPosition": [0.098, 0, -0.457] + }, + { + "zeroedRotations": [ + {"axis": "x", "degrees": 90}, + {"axis": "z", "degrees": 90} + ], + "zeroedPosition": [-0.252, 0, -0.207] + }, + { + "zeroedRotations": [ + {"axis": "x", "degrees": 90}, + {"axis": "z", "degrees": 90} + ], + "zeroedPosition": [-0.252, 0, -0.207] + } + ] +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 34515a6..80edcd5 100755 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -18,8 +18,8 @@ public class VisionConstants { // Transforms public static final Transform3d frontTransform = new Transform3d( - new Translation3d(Inches.of(0.125), Meters.of(-0.2032), Inches.of(20.7)), - new Rotation3d(Degrees.zero(), Degrees.of(-20), Degrees.zero()) + new Translation3d(Meters.of(-0.03), Meters.of(-0.003), Meters.of(0.526)), + new Rotation3d(Degrees.zero(), Degrees.of(-28), Degrees.zero()) ); public static final Transform3d backLeftTransform = new Transform3d( From b299ad7cddaba601db782d5d5eeb7e5e768c11b3 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 26 Feb 2026 11:18:51 -0800 Subject: [PATCH 3/3] Use latest cad camera positions --- .advantagescope/Robot_Relic2026/config.json | 25 ++++++++----------- .../subsystems/vision/VisionConstants.java | 6 ++--- 2 files changed, 13 insertions(+), 18 deletions(-) diff --git a/.advantagescope/Robot_Relic2026/config.json b/.advantagescope/Robot_Relic2026/config.json index 47d03e4..37f1f05 100644 --- a/.advantagescope/Robot_Relic2026/config.json +++ b/.advantagescope/Robot_Relic2026/config.json @@ -13,19 +13,14 @@ "rotations": [ { "axis": "y", - "degrees": -28 + "degrees": -30 } ], "position": [ - -0.03, - -0.003, + 0.0, + 0.0, 0.526 - ], - "resolution": [ - 1280, - 960 - ], - "fov": 83 + ] }, { "name": "Back Left Camera", @@ -40,9 +35,9 @@ } ], "position": [ - -0.155, - 0.318, - 0.213 + -0.160, + 0.322, + 0.216 ], "resolution": [ 1280, @@ -63,9 +58,9 @@ } ], "position": [ - -0.155, - -0.318, - 0.213 + -0.162, + -0.322, + 0.216 ], "resolution": [ 1280, diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 80edcd5..71758c3 100755 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -18,17 +18,17 @@ public class VisionConstants { // Transforms public static final Transform3d frontTransform = new Transform3d( - new Translation3d(Meters.of(-0.03), Meters.of(-0.003), Meters.of(0.526)), + new Translation3d(Meters.of(0.0), Meters.of(0.0), Meters.of(0.526)), new Rotation3d(Degrees.zero(), Degrees.of(-28), Degrees.zero()) ); public static final Transform3d backLeftTransform = new Transform3d( - new Translation3d(Meters.of(-0.155), Meters.of(0.318), Meters.of(0.213)), + new Translation3d(Meters.of(-0.160), Meters.of(0.322), Meters.of(0.216)), new Rotation3d(Degrees.zero(), Degrees.of(-30), Degrees.of(150)) ); public static final Transform3d backRightTransform = new Transform3d( - new Translation3d(Meters.of(-0.155), Meters.of(-0.318), Meters.of(0.213)), + new Translation3d(Meters.of(-0.162), Meters.of(-0.322), Meters.of(0.216)), new Rotation3d(Degrees.zero(), Degrees.of(-30), Degrees.of(210)) );