mega tag 2

FRC-2024.chor

@@ -27640,7 +27640,7 @@
                 }
             ],
             "eventMarkers": [],
-            "isTrajectoryStale": false
+            "isTrajectoryStale": true
         }
     },
     "splitTrajectoriesAtStopPoints": true,

src/main/java/org/codeorange/frc2024/robot/Robot.java

@@ -773,6 +773,10 @@ public static Climber getClimber() {
         return climber;
     }
 
+    public static Vision getVision() {
+        return vision;
+    }
+
     public static BlinkinLEDController getBlinkin() {
         return blinkin;
     }

src/main/java/org/codeorange/frc2024/subsystem/drive/Drive.java

@@ -15,7 +15,9 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.codeorange.frc2024.robot.Robot;
 import org.codeorange.frc2024.subsystem.AbstractSubsystem;
+import org.codeorange.frc2024.utility.Alert;
 import org.codeorange.frc2024.utility.ControllerDriveInputs;
+import org.codeorange.frc2024.utility.LimelightHelpers;
 import org.codeorange.frc2024.utility.MathUtil;
 import org.codeorange.frc2024.utility.swerve.SecondOrderModuleState;
 import org.codeorange.frc2024.utility.swerve.SecondOrderKinematics;
@@ -34,6 +36,11 @@ public class Drive extends AbstractSubsystem {
     static final Lock odometryLock = new ReentrantLock();
     public static final double SPEAKER_ANGLE_OFFSET = Units.degreesToRadians(4);
 
+    private final Alert odomAlert = new Alert("ODOMETRY WENT TO NAN!!!!", Alert.AlertType.ERROR);
+    {
+        odomAlert.set(false);
+    }
+
     final GyroIO gyroIO;
     private final GyroInputsAutoLogged gyroInputs = new GyroInputsAutoLogged();
     private final ModuleIO[] moduleIO;
@@ -133,6 +140,24 @@ public synchronized void update() {
         }
 
         realField.setRobotPose(getPose());
+
+        if(Double.isNaN(getPose().getX()) || Double.isNaN(getPose().getY()) || Double.isNaN(getPose().getRotation().getRadians())) {
+            poseEstimator.resetPosition(
+                    gyroInputs.rotation2d,
+                    getModulePositions(),
+                    new Pose2d(0, 0, gyroInputs.rotation2d)
+            );
+            odomAlert.set(true);
+        }
+
+        Robot.getVision().updateBotOrientation(
+                Units.radiansToDegrees(gyroInputs.yawPositionRad),
+                Units.radiansToDegrees(gyroInputs.yawVelocityRadPerSec),
+                Units.radiansToDegrees(gyroInputs.pitchPositionRad),
+                Units.radiansToDegrees(gyroInputs.pitchVelocityRadPerSec),
+                Units.radiansToDegrees(gyroInputs.rollPositionRad),
+                Units.radiansToDegrees(gyroInputs.rollVelocityRadPerSec)
+        );
     }
 
     public synchronized void setBrakeMode(boolean brakeMode) {

src/main/java/org/codeorange/frc2024/subsystem/drive/GyroIO.java

@@ -10,6 +10,10 @@ class GyroInputs {
         public boolean connected = false;
         public double yawPositionRad = 0.0;
         public double yawVelocityRadPerSec = 0.0;
+        public double pitchPositionRad = 0.0;
+        public double pitchVelocityRadPerSec = 0.0;
+        public double rollPositionRad = 0.0;
+        public double rollVelocityRadPerSec = 0.0;
 
         public Rotation3d rotation3d = new Rotation3d();
 

src/main/java/org/codeorange/frc2024/subsystem/drive/GyroIOPigeon2.java

@@ -17,6 +17,10 @@ public class GyroIOPigeon2 implements GyroIO {
 
     private final StatusSignal<Double> yawPositionDeg;
     private final StatusSignal<Double> yawVelocityDegPerSec;
+    private final StatusSignal<Double> pitchPositionDeg;
+    private final StatusSignal<Double> pitchVelocityDegPerSec;
+    private final StatusSignal<Double> rollPositionDeg;
+    private final StatusSignal<Double> rollVelocityDegPerSec;
     private final Queue<Double> yawPositionQueue;
     private final Queue<Double> yawTimestampQueue;
     private final StatusSignal<Double> uptime;
@@ -26,11 +30,21 @@ public class GyroIOPigeon2 implements GyroIO {
     public GyroIOPigeon2() {
         pigeon = new Pigeon2(Ports.PIGEON, CAN_BUS);
 
+        var config = new Pigeon2Configuration();
+
+        config.MountPose.MountPoseYaw = -94;
+        config.MountPose.MountPosePitch = 0;
+        config.MountPose.MountPoseRoll = 0;
+
         OrangeUtility.betterCTREConfigApply(pigeon, new Pigeon2Configuration());
         pigeon.reset();
 
         yawPositionDeg = pigeon.getYaw();
         yawVelocityDegPerSec = pigeon.getAngularVelocityZWorld();
+        pitchPositionDeg = pigeon.getPitch();
+        pitchVelocityDegPerSec = pigeon.getAngularVelocityXWorld();
+        rollPositionDeg = pigeon.getRoll();
+        rollVelocityDegPerSec = pigeon.getAngularVelocityYWorld();
         uptime = pigeon.getUpTime();
 
         BaseStatusSignal.setUpdateFrequencyForAll(ODOMETRY_REFRESH_HZ, yawPositionDeg);
@@ -42,7 +56,7 @@ public GyroIOPigeon2() {
 
     @Override
     public void updateInputs(GyroInputs inputs) {
-        BaseStatusSignal.refreshAll(uptime, yawPositionDeg, yawVelocityDegPerSec);
+        BaseStatusSignal.refreshAll(uptime, yawPositionDeg, yawVelocityDegPerSec, pitchPositionDeg, pitchVelocityDegPerSec, rollPositionDeg, rollVelocityDegPerSec);
         inputs.connected = uptime.getValue() > 0;
         /*
          * X-axis points forward
@@ -52,6 +66,10 @@ public void updateInputs(GyroInputs inputs) {
 
         inputs.yawPositionRad = Units.degreesToRadians(yawPositionDeg.getValue()); // counterclockwise positive
         inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocityDegPerSec.getValue());
+        inputs.pitchPositionRad = Units.degreesToRadians(pitchPositionDeg.getValue()); // counterclockwise positive
+        inputs.pitchVelocityRadPerSec = Units.degreesToRadians(pitchVelocityDegPerSec.getValue());
+        inputs.rollPositionRad = Units.degreesToRadians(rollPositionDeg.getValue()); // counterclockwise positive
+        inputs.rollVelocityRadPerSec = Units.degreesToRadians(rollVelocityDegPerSec.getValue());
 
         inputs.rotation3d = pigeon.getRotation3d();
         inputs.rotation2d = pigeon.getRotation2d();

src/main/java/org/codeorange/frc2024/subsystem/elevator/ElevatorIOTalonFX.java

@@ -74,7 +74,7 @@ public void setPosition(double targetPosition) {
     }
 
     public void updateInputs(ElevatorInputs inputs) {
-        BaseStatusSignal.refreshAll(leadMotorPosition, leadMotorVelocity, leadMotorVoltage, leadMotorAmps, leadMotorTemp, followMotorPosition, followMotorVoltage, followMotorAmps, followMotorTemp);
+        BaseStatusSignal.refreshAll(leadMotorPosition, leadMotorVelocity, leadMotorVoltage, leadMotorAmps, leadMotorTemp, followMotorPosition, followMotorVelocity, followMotorVoltage, followMotorAmps, followMotorTemp);
 
         inputs.leadMotorPosition = leadMotorPosition.getValue();
         inputs.leadMotorVelocity = leadMotorVelocity.getValue();

src/main/java/org/codeorange/frc2024/subsystem/vision/Vision.java

@@ -7,6 +7,7 @@
 import org.codeorange.frc2024.robot.Robot;
 import org.codeorange.frc2024.subsystem.AbstractSubsystem;
 import org.codeorange.frc2024.subsystem.drive.Drive;
+import org.codeorange.frc2024.utility.LimelightHelpers;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -22,7 +23,7 @@ public class Vision extends AbstractSubsystem {
     private static final String LL_BACK = "limelight-back";
     private static Drive drive;
     double fieldBorderMargin = 0.15;
-    double defaultXYStdev = 0.7;
+    double defaultXYStdev = 0.3;
 
 
     public static final LoggedDashboardChooser<Boolean> visionChooser;
@@ -54,6 +55,8 @@ public synchronized void update() {
     }
 
     private void processVisionData(VisionIO io, VisionIO.VisionInputs inputs) {
+        if(!inputs.hasTarget) return;
+
         if(unconditionallyTrustVision.get()) {
             drive.addVisionMeasurement(
                     inputs.botPose2d,
@@ -79,16 +82,22 @@ private void processVisionData(VisionIO io, VisionIO.VisionInputs inputs) {
         if(Math.abs(drive.getPose().getRotation().minus(inputs.botPose2d.getRotation()).getDegrees()) > 5) return;
 
         //exit if tags are too far in auto
-        if(inputs.avgDist > 4 && DriverStation.isAutonomous()) return;
+        // if(inputs.avgDist > 4 && DriverStation.isAutonomous()) return;
+
 
         Logger.recordOutput("Vision/" + io.getName() + "/Accepted Pose", inputs.botPose2d);
 
-        //scale stdevs with square of distance and tag count
-        double xyStdev = defaultXYStdev * inputs.avgDist * inputs.avgDist / inputs.tagCount / inputs.tagCount;
-        if(DriverStation.isAutonomousEnabled()) xyStdev *= inputs.avgDist;
+        //scale stdevs with ??
+        double xyStdev = defaultXYStdev * inputs.avgDist;
 
         Logger.recordOutput("Vision/" + io.getName() + "/XY Standard Deviations", xyStdev);
         io.getDashboardField().setRobotPose(inputs.botPose2d);
         drive.addVisionMeasurement(inputs.botPose2d, inputs.timestamp, VecBuilder.fill(xyStdev, xyStdev, 9999999));
     }
+
+    public void updateBotOrientation(double yaw, double yawVel, double pitch, double pitchVel, double roll, double rollVel) {
+        for(VisionIO io : visionIO) {
+            LimelightHelpers.SetRobotOrientation(io.getName(), yaw, yawVel, pitch, pitchVel, roll, rollVel);
+        }
+    }
 }

src/main/java/org/codeorange/frc2024/subsystem/vision/VisionIOLimelight.java

@@ -24,7 +24,7 @@ public VisionIOLimelight(String name) {
 
     @Override
     public void updateInputs(VisionInputs inputs) {
-        LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
+        LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName);
         var hb = LimelightHelpers.getLimelightNTDouble(limelightName, "hb");
         inputs.connected = (hb > 0) && !(Logger.getRealTimestamp() * 1e-6 > prevHBtimestamp + 0.5);
         visionAlert.set(!inputs.connected);

src/main/java/org/codeorange/frc2024/utility/LimelightHelpers.java

@@ -1,4 +1,4 @@
-//LimelightHelpers v1.3.1 (March 4, 2024)
+//LimelightHelpers v1.5.0 (March 27, 2024)
 
 package org.codeorange.frc2024.utility;
 
@@ -385,6 +385,27 @@ public LimelightResults() {
 
     }
 
+    public static class RawFiducial {
+        public int id;
+        public double txnc;
+        public double tync;
+        public double ta;
+        public double distToCamera;
+        public double distToRobot;
+        public double ambiguity;
+
+
+        public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) {
+            this.id = id;
+            this.txnc = txnc;
+            this.tync = tync;
+            this.ta = ta;
+            this.distToCamera = distToCamera;
+            this.distToRobot = distToRobot;
+            this.ambiguity = ambiguity;
+        }
+    }
+
     public static class PoseEstimate {
         public Pose2d pose;
         public double timestampSeconds;
@@ -393,16 +414,20 @@ public static class PoseEstimate {
         public double tagSpan;
         public double avgTagDist;
         public double avgTagArea;
+        public RawFiducial[] rawFiducials;
 
+        public PoseEstimate(Pose2d pose, double timestampSeconds, double latency,
+                            int tagCount, double tagSpan, double avgTagDist,
+                            double avgTagArea, RawFiducial[] rawFiducials) {
 
-        public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea) {
             this.pose = pose;
             this.timestampSeconds = timestampSeconds;
             this.latency = latency;
             this.tagCount = tagCount;
             this.tagSpan = tagSpan;
             this.avgTagDist = avgTagDist;
             this.avgTagArea = avgTagArea;
+            this.rawFiducials = rawFiducials;
         }
     }
 
@@ -462,7 +487,65 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr
         double tagArea = extractBotPoseEntry(poseArray,10);
         //getlastchange() in microseconds, ll latency in milliseconds
         var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0);
-        return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea);
+
+
+        RawFiducial[] rawFiducials = new RawFiducial[tagCount];
+        int valsPerFiducial = 7;
+        int expectedTotalVals = 11 + valsPerFiducial*tagCount;
+
+        if (poseArray.length != expectedTotalVals) {
+            // Don't populate fiducials
+        }
+        else{
+            for(int i = 0; i < tagCount; i++) {
+                int baseIndex = 11 + (i * valsPerFiducial);
+                int id = (int)poseArray[baseIndex];
+                double txnc = poseArray[baseIndex + 1];
+                double tync = poseArray[baseIndex + 2];
+                double ta = poseArray[baseIndex + 3];
+                double distToCamera = poseArray[baseIndex + 4];
+                double distToRobot = poseArray[baseIndex + 5];
+                double ambiguity = poseArray[baseIndex + 6];
+                rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+            }
+        }
+
+        return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials);
+    }
+
+    private static void printPoseEstimate(PoseEstimate pose) {
+        if (pose == null) {
+            System.out.println("No PoseEstimate available.");
+            return;
+        }
+
+        System.out.printf("Pose Estimate Information:%n");
+        System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds);
+        System.out.printf("Latency: %.3f ms%n", pose.latency);
+        System.out.printf("Tag Count: %d%n", pose.tagCount);
+        System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
+        System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
+        System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
+        System.out.println();
+
+        if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
+            System.out.println("No RawFiducials data available.");
+            return;
+        }
+
+        System.out.println("Raw Fiducials Details:");
+        for (int i = 0; i < pose.rawFiducials.length; i++) {
+            RawFiducial fiducial = pose.rawFiducials[i];
+            System.out.printf(" Fiducial #%d:%n", i + 1);
+            System.out.printf("  ID: %d%n", fiducial.id);
+            System.out.printf("  TXNC: %.2f%n", fiducial.txnc);
+            System.out.printf("  TYNC: %.2f%n", fiducial.tync);
+            System.out.printf("  TA: %.2f%n", fiducial.ta);
+            System.out.printf("  Distance to Camera: %.2f meters%n", fiducial.distToCamera);
+            System.out.printf("  Distance to Robot: %.2f meters%n", fiducial.distToRobot);
+            System.out.printf("  Ambiguity: %.2f%n", fiducial.ambiguity);
+            System.out.println();
+        }
     }
 
     public static NetworkTable getLimelightNTTable(String tableName) {
@@ -675,6 +758,17 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
         return getBotPoseEstimate(limelightName, "botpose_wpiblue");
     }
 
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
+     * alliance
+     *
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue");
+    }
+
     /**
      * Gets the Pose2d for easy use with Odometry vision pose estimator
      * (addVisionMeasurement)
@@ -699,6 +793,16 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
         return getBotPoseEstimate(limelightName, "botpose_wpired");
     }
 
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpired");
+    }
+
     /**
      * Gets the Pose2d for easy use with Odometry vision pose estimator
      * (addVisionMeasurement)
@@ -782,6 +886,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c
         setLimelightNTDoubleArray(limelightName, "crop", entries);
     }
 
+    public static void SetRobotOrientation(String limelightName, double yaw, double yawRate,
+                                           double pitch, double pitchRate,
+                                           double roll, double rollRate) {
+
+        double[] entries = new double[6];
+        entries[0] = yaw;
+        entries[1] = yawRate;
+        entries[2] = pitch;
+        entries[3] = pitchRate;
+        entries[4] = roll;
+        entries[5] = rollRate;
+        setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
+    }
+
+    public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
+        double[] validIDsDouble = new double[validIDs.length];
+        for (int i = 0; i < validIDs.length; i++) {
+            validIDsDouble[i] = validIDs[i];
+        }
+        setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
+    }
+
     public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) {
         double[] entries = new double[6];
         entries[0] = forward;