Robodox-599 · Meerlikeskeer · Dec 6, 2024
diff --git a/2025_PreseasonSwerve/src/main/java/frc/robot/FieldConstants.java b/2025_PreseasonSwerve/src/main/java/frc/robot/FieldConstants.java
@@ -0,0 +1,144 @@
+package frc.robot;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+
+/**
+ * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets
+ * of corners start in the lower left moving clockwise. <b>All units in Meters</b> <br>
+ * <br>
+ *
+ * <p>All translations and poses are stored with the origin at the rightmost point on the BLUE
+ * ALLIANCE wall.<br>
+ * <br>
+ * Length refers to the <i>x</i> direction (as described by wpilib) <br>
+ * Width refers to the <i>y</i> direction (as described by wpilib)
+ */
+public class FieldConstants {
+  public static final double fieldLength = Units.inchesToMeters(651.223);
+  public static final double fieldWidth = Units.inchesToMeters(323.277);
+  public static final double wingX = Units.inchesToMeters(229.201);
+  public static final double podiumX = Units.inchesToMeters(126.75);
+  public static final double startingLineX = Units.inchesToMeters(74.111);
+
+  public static final Translation2d ampCenter =
+      new Translation2d(Units.inchesToMeters(72.455), fieldWidth);
+
+  private static Pose2d ampPose = new Pose2d(ampCenter, Rotation2d.fromDegrees(-90));
+
+  /** Staging locations for each note */
+  public static final class StagingLocations {
+    public static final double centerlineX = fieldLength / 2.0;
+
+    // need to update
+    public static final double centerlineFirstY = Units.inchesToMeters(29.638);
+    public static final double centerlineSeparationY = Units.inchesToMeters(66);
+    public static final double spikeX = Units.inchesToMeters(114);
+    // need
+    public static final double spikeFirstY = Units.inchesToMeters(161.638);
+    public static final double spikeSeparationY = Units.inchesToMeters(57);
+
+    public static final Translation2d[] centerlineTranslations = new Translation2d[5];
+    public static final Translation2d[] spikeTranslations = new Translation2d[3];
+
+    static {
+      for (int i = 0; i < centerlineTranslations.length; i++) {
+        centerlineTranslations[i] =
+            new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY));
+      }
+    }
+
+    static {
+      for (int i = 0; i < spikeTranslations.length; i++) {
+        spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY));
+      }
+    }
+  }
+
+  /** Each corner of the speaker * */
+  public static final class Speaker {
+
+    // corners (blue alliance origin)
+    public static final Translation3d topRightSpeaker =
+        new Translation3d(
+            Units.inchesToMeters(18.055),
+            Units.inchesToMeters(238.815),
+            Units.inchesToMeters(83.091));
+
+    public static final Translation3d topLeftSpeaker =
+        new Translation3d(
+            Units.inchesToMeters(18.055),
+            Units.inchesToMeters(197.765),
+            Units.inchesToMeters(83.091));
+
+    public static final Translation3d bottomRightSpeaker =
+        new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324));
+    public static final Translation3d bottomLeftSpeaker =
+        new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324));
+
+    /** Center of the speaker opening (blue alliance) */
+    public static final Translation3d centerSpeakerOpening =
+        bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5);
+  }
+
+  public static final Pose2d SpeakerPosition = new Pose2d(-0.2, (5 + 6.12) / 2, new Rotation2d(0));
+  // 197.765
+  public static final class Subwoofer {
+    public static final Pose2d ampFaceCorner =
+        new Pose2d(
+            Units.inchesToMeters(35.775),
+            Units.inchesToMeters(239.366),
+            Rotation2d.fromDegrees(-120));
+
+    public static final Pose2d sourceFaceCorner =
+        new Pose2d(
+            Units.inchesToMeters(35.775),
+            Units.inchesToMeters(197.466),
+            Rotation2d.fromDegrees(120));
+
+    public static final Pose2d centerFace =
+        new Pose2d(
+            Units.inchesToMeters(35.775),
+            Units.inchesToMeters(218.416),
+            Rotation2d.fromDegrees(180));
+  }
+
+  public static final class Stage {
+    public static final Pose2d podiumLeg =
+        new Pose2d(Units.inchesToMeters(126.75), Units.inchesToMeters(161.638), new Rotation2d());
+    public static final Pose2d ampLeg =
+        new Pose2d(
+            Units.inchesToMeters(220.873),
+            Units.inchesToMeters(212.425),
+            Rotation2d.fromDegrees(-30));
+    public static final Pose2d sourceLeg =
+        new Pose2d(
+            Units.inchesToMeters(220.873),
+            Units.inchesToMeters(110.837),
+            Rotation2d.fromDegrees(30));
+
+    public static final Pose2d centerPodiumAmpChain =
+        new Pose2d(
+            podiumLeg.getTranslation().interpolate(ampLeg.getTranslation(), 0.5),
+            Rotation2d.fromDegrees(120.0));
+    public static final Pose2d centerAmpSourceChain =
+        new Pose2d(
+            ampLeg.getTranslation().interpolate(sourceLeg.getTranslation(), 0.5), new Rotation2d());
+    public static final Pose2d centerSourcePodiumChain =
+        new Pose2d(
+            sourceLeg.getTranslation().interpolate(podiumLeg.getTranslation(), 0.5),
+            Rotation2d.fromDegrees(240.0));
+    public static final Pose2d center =
+        new Pose2d(Units.inchesToMeters(192.55), Units.inchesToMeters(161.638), new Rotation2d());
+    public static final double centerToChainDistance =
+        center.getTranslation().getDistance(centerPodiumAmpChain.getTranslation());
+  }
+    public static AprilTagFieldLayout aprilTagLayout =
+      AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+
+}
diff --git a/2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/vision/Vision.java b/2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -0,0 +1,171 @@
+
+package frc.robot.subsystems.vision;
+
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.FieldConstants;
+
+import java.util.LinkedList;
+import java.util.List;
+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;
+
+  public Vision(VisionConsumer consumer, VisionIO... io) {
+    this.consumer = consumer;
+    this.io = io;
+
+    // Initialize inputs
+    this.inputs = new VisionIOInputsAutoLogged[io.length];
+    for (int i = 0; i < inputs.length; i++) {
+      inputs[i] = new VisionIOInputsAutoLogged();
+    }
+
+    // Initialize disconnected alerts
+    this.disconnectedAlerts = new Alert[io.length];
+    for (int i = 0; i < inputs.length; i++) {
+      disconnectedAlerts[i] =
+          new Alert(
+              io[i].getName() + " is disconnected.", AlertType.kWarning);
+    }
+  }
+
+  /**
+   * Returns the X angle to the best target, which can be used for simple servoing with vision.
+   *
+   * @param cameraIndex The index of the camera to use.
+   */
+  public Rotation2d getTargetX(int cameraIndex) {
+    return inputs[cameraIndex].latestTargetAngle.targetX();
+  }
+
+  @Override
+  public void periodic() {
+    for (int i = 0; i < io.length; i++) {
+      io[i].updateInputs(inputs[i]);
+      Logger.processInputs("Vision/" + io[i].getName(), inputs[i]);
+    }
+
+    // Initialize logging values
+    List<Pose3d> allTagPoses = new LinkedList<>();
+    List<Pose3d> allRobotPoses = new LinkedList<>();
+    List<Pose3d> allRobotPosesAccepted = new LinkedList<>();
+    List<Pose3d> allRobotPosesRejected = new LinkedList<>();
+
+    // Loop over cameras
+    for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) {
+      // Update disconnected alert
+      disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].cameraConnected);
+
+      // Initialize logging values
+      List<Pose3d> tagPoses = new LinkedList<>();
+      List<Pose3d> robotPoses = new LinkedList<>();
+      List<Pose3d> robotPosesAccepted = new LinkedList<>();
+      List<Pose3d> robotPosesRejected = new LinkedList<>();
+
+      // Add tag poses
+      for (int tagId : inputs[cameraIndex].aprilTagIds) {
+        var tagPose = FieldConstants.aprilTagLayout.getTagPose(tagId);
+        if (tagPose.isPresent()) {
+          tagPoses.add(tagPose.get());
+        }
+      }
+
+      // Loop over pose observations
+      for (var observation : inputs[cameraIndex].poseObservations) {
+        // Check whether to reject pose
+        boolean rejectPose =
+            observation.tagCount() == 0 // Must have at least one tag
+                || (observation.tagCount() == 1
+                    && observation.ambiguity() > io[cameraIndex].getVisionConstants().maxAmbiguity() ) // Cannot be high ambiguity
+                || Math.abs(observation.observedPose().getZ())
+                    > io[cameraIndex].getVisionConstants().maxZError() // Must have realistic Z coordinate
+                // Must be within the field boundaries
+                || observation.observedPose().getX() < 0.0
+                || observation.observedPose().getX() > FieldConstants.aprilTagLayout.getFieldLength()
+                || observation.observedPose().getY() < 0.0
+                || observation.observedPose().getY() > FieldConstants.aprilTagLayout.getFieldWidth();
+
+        // Add pose to log
+        robotPoses.add(observation.observedPose());
+        if (rejectPose) {
+          robotPosesRejected.add(observation.observedPose());
+        } else {
+          robotPosesAccepted.add(observation.observedPose());
+        }
+
+        // Skip if rejected
+        if (rejectPose) {
+          continue;
+        }
+
+        // Calculate standard deviations
+        double stdDevFactor =
+            Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
+        double linearStdDev = io[cameraIndex].getVisionConstants().linearStdDevBaseline() * stdDevFactor;
+        double angularStdDev = io[cameraIndex].getVisionConstants().angularStdDevBaseline() * stdDevFactor;
+
+        linearStdDev *= io[cameraIndex].getVisionConstants().cameraStdDevFactor();
+        angularStdDev *= io[cameraIndex].getVisionConstants().angularStdDevBaseline();
+
+
+        // Send vision observation
+        consumer.accept(
+            observation.observedPose().toPose2d(),
+            observation.timestamp(),
+            VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev));
+      }
+
+      // Log camera datadata
+      Logger.recordOutput(
+          "Vision/" + io[cameraIndex].getName() + "/TagPoses",
+          tagPoses.toArray(new Pose3d[tagPoses.size()]));
+      Logger.recordOutput(
+          "Vision/" + io[cameraIndex].getName() + "/RobotPoses",
+          robotPoses.toArray(new Pose3d[robotPoses.size()]));
+      Logger.recordOutput(
+          "Vision/" + Integer.toString(cameraIndex) + "/AcceptedRobotPoses",
+          robotPosesAccepted.toArray(new Pose3d[robotPosesAccepted.size()]));
+      Logger.recordOutput(
+          "Vision/" + Integer.toString(cameraIndex) + "/RejectedRobotPoses",
+          robotPosesRejected.toArray(new Pose3d[robotPosesRejected.size()]));
+      allTagPoses.addAll(tagPoses);
+      allRobotPoses.addAll(robotPoses);
+      allRobotPosesAccepted.addAll(robotPosesAccepted);
+      allRobotPosesRejected.addAll(robotPosesRejected);
+    }
+
+    // Log summary data
+    Logger.recordOutput(
+        "Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()]));
+    Logger.recordOutput(
+        "Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[allRobotPoses.size()]));
+    Logger.recordOutput(
+        "Vision/Summary/AcceptedRobotPoses",
+        allRobotPosesAccepted.toArray(new Pose3d[allRobotPosesAccepted.size()]));
+    Logger.recordOutput(
+        "Vision/Summary/RejectedRobotPoses",
+        allRobotPosesRejected.toArray(new Pose3d[allRobotPosesRejected.size()]));
+  }
+
+  @FunctionalInterface
+  public static interface VisionConsumer {
+    public void accept(
+        Pose2d visionRobotPoseMeters,
+        double timestampSeconds,
+        Matrix<N3, N1> visionMeasurementStdDevs);
+  }
+}
diff --git a/2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/2025_PreseasonSwerve/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -0,0 +1,54 @@
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+/**
+ * Represents constants and parameters used for vision 
+ * This record defines the camera configuration, its transform relative to the robot, and standard deviation factors for pose estimation.
+
+ * @param cameraName the name or identifier of the camera
+ * @param robotToCameraTransform3d the 3D transform from the robot's coordinate frame to the camera's
+ * @param cameraStdDevFactor a scaling factor applied to the standard deviation of the camera measurements.
+ *  If value is 1, then no factor will be applied to the standard deviation of the camera measurements
+ */
+public record VisionConstants(
+    String cameraName,
+    Transform3d robotToCameraTransform3d,
+    double cameraStdDevFactor
+) {
+
+    /** 
+     * The maximum non-Filtered ambiguity in a pose observation. Observations with ambiguity higher
+     * than this threshold will be filtered out.
+     */
+    public double maxAmbiguity() {
+        return 0.3;
+    }
+
+    /** 
+     * The maximum allowed Z-axis error in a pose observation (in meters). Observations
+     * greater than this error will be rejected.
+     */
+    public double maxZError() {
+        return 0.75;
+    }
+
+    /** 
+     * The baseline standard deviation for linear measurements at 1 meter distance
+     * and 1 detected tag. This value is adjusted dynamically (based on actual
+     * distance and the number of detected tags), in the Vision class. 
+     */
+    public double linearStdDevBaseline() {
+        return 0.02;
+    }
+
+    /** 
+     * The baseline standard deviation for angular measurements at 1 meter distance
+     * and 1 detected tag. This value is adjusted dynamically (based on actual
+     * distance and the number of detected tags), in the Vision class. 
+     */
+    public double angularStdDevBaseline() {
+        return 0.06;
+    }
+}
+