Team3256 · memmayu · Mar 26, 2025 · Mar 26, 2025
diff --git a/src/main/deploy/choreo/C-Source2.traj b/src/main/deploy/choreo/C-Source2.traj
@@ -13,7 +13,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"C.x", "val":3.7114241123199463}, "y":{"exp":"C.y", "val":2.9666056632995605}, "heading":{"exp":"C.heading", "val":2.606637673727935}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"C.x", "val":3.657078504562378}, "y":{"exp":"C.y", "val":2.9981400966644287}, "heading":{"exp":"C.heading", "val":2.606637673727935}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

diff --git a/src/main/deploy/choreo/D-Source2.traj b/src/main/deploy/choreo/D-Source2.traj
@@ -13,7 +13,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"D.x", "val":4.044410705566406}, "y":{"exp":"D.y", "val":2.776327610015869}, "heading":{"exp":"D.heading", "val":2.606637673727935}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"D.x", "val":3.9831042289733887}, "y":{"exp":"D.y", "val":2.799689769744873}, "heading":{"exp":"D.heading", "val":2.606637673727935}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

diff --git a/src/main/deploy/choreo/H-Source2.traj b/src/main/deploy/choreo/H-Source2.traj
@@ -14,7 +14,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"H.x", "val":5.785916328430176}, "y":{"exp":"H.y", "val":4.243002891540527}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"H.x", "val":5.8544793128967285}, "y":{"exp":"H.y", "val":4.183961868286133}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"5.847537517547607 m", "val":5.847537517547607}, "y":{"exp":"3.057382345199585 m", "val":3.057382345199585}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
     {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[

diff --git a/src/main/deploy/choreo/MID-H.traj b/src/main/deploy/choreo/MID-H.traj
@@ -14,7 +14,7 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"7.228333473205566 m", "val":7.228333473205566}, "y":{"exp":"4.211480140686035 m", "val":4.211480140686035}, "heading":{"exp":"270 deg", "val":4.71238898038469}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"H.x", "val":5.785916328430176}, "y":{"exp":"H.y", "val":4.243002891540527}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"H.x", "val":5.8544793128967285}, "y":{"exp":"H.y", "val":4.183961868286133}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

diff --git a/src/main/deploy/choreo/New Path (5).traj b/src/main/deploy/choreo/New Path (5).traj
@@ -0,0 +1,29 @@
+{
+ "name":"New Path (5)",
+ "version":1,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"4.507959842681885 m", "val":4.507959842681885}, "y":{"exp":"4.0053911209106445 m", "val":4.0053911209106445}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.465804576873779 m", "val":6.465804576873779}, "y":{"exp":"4.026000022888184 m", "val":4.026000022888184}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/Source2-C.traj b/src/main/deploy/choreo/Source2-C.traj
@@ -17,7 +17,7 @@
   "waypoints":[
     {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"2.75620436668396 m", "val":2.75620436668396}, "y":{"exp":"2.418506622314453 m", "val":2.418506622314453}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"C.x", "val":3.7114241123199463}, "y":{"exp":"C.y", "val":2.9666056632995605}, "heading":{"exp":"C.heading", "val":2.606637673727935}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"C.x", "val":3.657078504562378}, "y":{"exp":"C.y", "val":2.9981400966644287}, "heading":{"exp":"C.heading", "val":2.606637673727935}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

diff --git a/src/main/deploy/choreo/Source2-D.traj b/src/main/deploy/choreo/Source2-D.traj
@@ -14,7 +14,7 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"D.x", "val":4.044410705566406}, "y":{"exp":"D.y", "val":2.776327610015869}, "heading":{"exp":"D.heading", "val":2.606637673727935}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"D.x", "val":3.9831042289733887}, "y":{"exp":"D.y", "val":2.799689769744873}, "heading":{"exp":"D.heading", "val":2.606637673727935}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

diff --git a/src/main/deploy/choreo/Source2-H.traj b/src/main/deploy/choreo/Source2-H.traj
@@ -16,7 +16,7 @@
   "waypoints":[
     {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"5.8681464195251465 m", "val":5.8681464195251465}, "y":{"exp":"2.8306844234466553 m", "val":2.8306844234466553}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"H.x", "val":5.785916328430176}, "y":{"exp":"H.y", "val":4.243002891540527}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"H.x", "val":5.8544793128967285}, "y":{"exp":"H.y", "val":4.183961868286133}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -43,10 +43,7 @@
 import frc.robot.subsystems.endeffector.EndEffectorIOTalonFX;
 import frc.robot.subsystems.swerve.CommandSwerveDrivetrain;
 import frc.robot.subsystems.swerve.generated.TunerConstants;
-import frc.robot.subsystems.vision.Vision;
-import frc.robot.subsystems.vision.VisionConstants;
-import frc.robot.subsystems.vision.VisionIOPhotonVision;
-import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
+import frc.robot.subsystems.vision.*;
 import frc.robot.utils.MappedXboxController;
 import frc.robot.utils.autoaim.AutoAim;
 import frc.robot.utils.autoaim.CoralTargets;
@@ -88,18 +85,29 @@ public class RobotContainer {
   private final Vision vision =
       new Vision(
           drivetrain::addPhotonEstimate,
+          drivetrain::shouldUseTrig,
           Utils.isSimulation()
               ? new VisionIOPhotonVisionSim(
                   VisionConstants.leftCam,
                   VisionConstants.robotToLeftCam,
                   () -> drivetrain.getState().Pose)
-              : new VisionIOPhotonVision(VisionConstants.leftCam, VisionConstants.robotToLeftCam),
+              : new VisionIOPhotonVisionTrig(
+                  VisionConstants.leftCam,
+                  VisionConstants.robotToLeftCam,
+                  () -> drivetrain.getPigeon2().getRotation2d(),
+                  Utils::getCurrentTimeSeconds,
+                  drivetrain::shouldUseTrig),
           Utils.isSimulation()
               ? new VisionIOPhotonVisionSim(
                   VisionConstants.rightCam,
                   VisionConstants.robotToRightCam,
                   () -> drivetrain.getState().Pose)
-              : new VisionIOPhotonVision(VisionConstants.rightCam, VisionConstants.robotToRightCam),
+              : new VisionIOPhotonVisionTrig(
+                  VisionConstants.rightCam,
+                  VisionConstants.robotToRightCam,
+                  () -> drivetrain.getPigeon2().getRotation2d(),
+                  Utils::getCurrentTimeSeconds,
+                  drivetrain::shouldUseTrig),
           Utils.isSimulation()
               ? new VisionIOPhotonVisionSim(
                   VisionConstants.backCam,

diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java
@@ -39,9 +39,11 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.FieldConstants;
 import frc.robot.drivers.QuestNav;
 import frc.robot.subsystems.swerve.generated.TunerConstants;
 import frc.robot.subsystems.swerve.generated.TunerConstants.TunerSwerveDrivetrain;
+import frc.robot.subsystems.vision.VisionConstants;
 import frc.robot.utils.LoggedTracer;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
@@ -177,6 +179,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
           SwerveConstants.frontRight,
           SwerveConstants.backLeft,
           SwerveConstants.backRight);
+
   /* WPILib Alerts start */
 
   private final Alert a_questNavNotConnected =
@@ -295,6 +298,11 @@ public void trajLogger(Trajectory<SwerveSample> sample, boolean isStart) {
     Logger.recordOutput(this.getClass().getSimpleName() + "/Choreo/TrajPoses", sample.getPoses());
   }
 
+  public boolean shouldUseTrig() {
+    return this.getState().Pose.getTranslation().getDistance(FieldConstants.Reef.center)
+        < VisionConstants.distanceToUseTrigMeters;
+  }
+
   /**
    * Creates a new auto factory for this drivetrain with the given trajectory logger.
    *

diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -18,17 +18,20 @@
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.utils.LoggedTracer;
+import java.util.function.BooleanSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class Vision extends SubsystemBase {
   private final VisionConsumer consumer;
   private final VisionIO[] io;
   private final VisionIOInputsAutoLogged[] inputs;
   private final Alert[] disconnectedAlerts;
+  private final BooleanSupplier shouldUseTrig;
 
-  public Vision(VisionConsumer consumer, VisionIO... io) {
+  public Vision(VisionConsumer consumer, BooleanSupplier shouldUseTrig, VisionIO... io) {
     this.consumer = consumer;
     this.io = io;
+    this.shouldUseTrig = shouldUseTrig;
 
     // Initialize inputs
     this.inputs = new VisionIOInputsAutoLogged[io.length];
@@ -65,6 +68,11 @@ public void periodic() {
 
       // Loop over pose observations
       for (var observation : inputs[cameraIndex].poseObservations) {
+
+        if (observation.type().equals(VisionIO.PoseObservationType.MULTITAG)
+            && shouldUseTrig.getAsBoolean()) {
+          continue;
+        }
         // Check whether to reject pose
         boolean rejectPose =
             observation.tagCount() == 0 // Must have at least one tag
@@ -87,6 +95,9 @@ public void periodic() {
         // Calculate standard deviations
         double stdDevFactor =
             Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
+        if (observation.type().equals(VisionIO.PoseObservationType.TRIG)) {
+          stdDevFactor /= 2;
+        }
         double linearStdDev = linearStdDevBaseline * stdDevFactor;
         double angularStdDev = angularStdDevBaseline * stdDevFactor;
         if (cameraIndex < cameraStdDevFactors.length) {

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -24,6 +24,10 @@ public class VisionConstants {
   public static String frontCam = "front";
   public static String backCam = "back";
 
+  public static int[] reefIds = new int[] {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22};
+
+  public static final double distanceToUseTrigMeters = 2.5;
+
   // Robot to camera transforms
   // (Not used by Limelight, configure in web UI instead)
   public static Transform3d robotToLeftCam =
@@ -64,14 +68,9 @@ public class VisionConstants {
   // (Adjust to trust some cameras more than others)
   public static double[] cameraStdDevFactors =
       new double[] {
-        1.5, // Camera 0
+        1, // Camera 0
         1, // Camera 1
         1.5,
         1.5 // Camera 3
       };
-
-  // Multipliers to apply for MegaTag 2 observations
-  public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve
-  public static double angularStdDevMegatag2Factor =
-      Double.POSITIVE_INFINITY; // No rotation data available
 }
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
@@ -8,7 +8,6 @@
 package frc.robot.subsystems.vision;
 
 import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface VisionIO {
@@ -19,9 +18,6 @@ public static class VisionIOInputs {
     public int[] tagIds = new int[0];
   }
 
-  /** Represents the angle to a simple target, not used for pose estimation. */
-  public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
-
   /** Represents a robot pose sample used for pose estimation. */
   public static record PoseObservation(
       double timestamp,
@@ -32,9 +28,8 @@ public static record PoseObservation(
       PoseObservationType type) {}
 
   public static enum PoseObservationType {
-    MEGATAG_1,
-    MEGATAG_2,
-    PHOTONVISION
+    MULTITAG,
+    TRIG
   }
 
   public default void updateInputs(VisionIOInputs inputs) {}

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
@@ -69,7 +69,7 @@ public void updateInputs(VisionIOInputs inputs) {
                 multitagResult.estimatedPose.ambiguity, // Ambiguity
                 multitagResult.fiducialIDsUsed.size(), // Tag count
                 totalTagDistance / result.targets.size(), // Average tag distance
-                PoseObservationType.PHOTONVISION)); // Observation type
+                PoseObservationType.MULTITAG)); // Observation type
 
       } else if (!result.targets.isEmpty()) { // Single tag result
         var target = result.targets.get(0);
@@ -95,7 +95,7 @@ public void updateInputs(VisionIOInputs inputs) {
                   target.poseAmbiguity, // Ambiguity
                   1, // Tag count
                   cameraToTarget.getTranslation().getNorm(), // Average tag distance
-                  PoseObservationType.PHOTONVISION)); // Observation type
+                  PoseObservationType.MULTITAG)); // Observation type
         }
       }
     }