Questnav

src/main/java/frc/robot/Constants.java

@@ -33,6 +33,7 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.numbers.N1;
@@ -309,5 +310,7 @@ public static class ClimbK {
             .withReverseSoftLimitEnable(true)
             .withReverseSoftLimitThreshold(minAngle);
     }
-
+    public static class QuestK {
+        public static final Transform3d ROBOT_TO_QUEST = new Transform3d(new Pose3d(0,0,0, new Rotation3d()), new Pose3d()); //! FIND
+    }
 }

src/main/java/frc/robot/Robot.java

@@ -52,20 +52,24 @@
 import frc.robot.subsystems.Climb;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Quest;
 import frc.robot.subsystems.Swerve;
-import frc.robot.subsystems.Vision;
 
 @Logged
 public class Robot extends TimedRobot {
 
     @Logged(name = "Vision")
-    private final Vision vision = new Vision();
+    private final Quest quest = new Quest();
     private final CommandXboxController xboxController = new CommandXboxController(ControllerK.xboxPort);
     @Logged(name = "Swerve")
-    private final Swerve swerve = new Swerve(vision::getVisionResults, () -> 
+    private final Swerve swerve = new Swerve(quest::getQuestResults, () -> 
         Math.abs(xboxController.getLeftX()) > ControllerK.overrideThreshold
         || Math.abs(xboxController.getLeftY()) > ControllerK.overrideThreshold
         || Math.abs(xboxController.getRightX()) > ControllerK.overrideThreshold);
+    // private final Swerve swerve = new Swerve(vision::getVisionResults, () -> 
+    //     Math.abs(xboxController.getLeftX()) > ControllerK.overrideThreshold
+    //     || Math.abs(xboxController.getLeftY()) > ControllerK.overrideThreshold
+    //     || Math.abs(xboxController.getRightX()) > ControllerK.overrideThreshold);
     @Logged(name = "Elevator")
     private final Elevator elevator = new Elevator();
     @Logged(name = "Intake")

src/main/java/frc/robot/subsystems/Quest.java

@@ -0,0 +1,110 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.Utils;
+
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.epilogue.NotLogged;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.QuestK;
+import gg.questnav.questnav.PoseFrame;
+import gg.questnav.questnav.QuestNav;
+import swervelib.SwerveDrive;
+
+@Logged
+public class Quest extends SubsystemBase {
+    QuestNav questNav = new QuestNav();
+
+    //private Swerve swerve = new Swerve(null, null);
+
+    private SwerveDrive swerveDrive;
+
+    //private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(null, null, null, null);
+
+    public void GetPose() {
+        // First, Declare our geometrical transform from the robot center to the Quest
+
+
+        // Get the latest pose data frames from the Quest
+        PoseFrame[] poseFrames = questNav.getAllUnreadPoseFrames();
+
+        if (poseFrames.length > 0) {
+                // Get the most recent Quest pose
+                Pose3d questPose = poseFrames[poseFrames.length - 1].questPose3d();
+
+                // Transform by the mount pose to get your robot pose
+                Pose3d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse());
+        }
+    }
+
+    public void SetPose() {
+        // First, Declare our geometrical transform from the robot center to the quest
+        // Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, z, yaw, pitch, and roll offsets here!*/ );
+
+        // Assume this is the requested reset pose
+        Pose3d robotPose = new Pose3d( /* Some pose data */ );
+
+        // Transform by the offset to get the Quest pose
+        Pose3d questPose = robotPose.transformBy(QuestK.ROBOT_TO_QUEST);
+
+        // Send the reset operation
+        questNav.setPose(questPose);
+    }
+        @NotLogged
+        Matrix<N3, N1> QUESTNAV_STD_DEVS =
+            VecBuilder.fill(
+                0.02, // Trust down to 2cm in X direction
+                0.02, // Trust down to 2cm in Y direction
+                0.035 // Trust down to 2 degrees rotational
+        );
+
+
+    @Override
+    public void periodic() {
+
+        if (questNav.isTracking()) {
+            // Get the latest pose data frames from the Quest
+            PoseFrame[] questFrames = questNav.getAllUnreadPoseFrames();
+
+            // Loop over the pose data frames and send them to the pose estimator 
+            for (PoseFrame questFrame : questFrames) {
+                // Get the pose of the Quest
+                Pose3d questPose = questFrame.questPose3d();
+                // Get timestamp for when the data was sent
+                double timestamp = questFrame.dataTimestamp();
+                // Transform by the mount pose to get your robot pose
+                Pose3d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse());
+
+                // Convert FPGA timestamp to CTRE's time domain using Phoenix 6 utility
+                double ctreTimestamp = Utils.fpgaToCurrentTime(timestamp);
+
+                // // You can put some sort of filtering here if you would like!
+
+                // // Add the measurement to our estimator
+                swerveDrive.addVisionMeasurement(robotPose.toPose2d(), timestamp, QUESTNAV_STD_DEVS);
+            };
+        }
+}
+    /*
+     Attempt to make a QuestNav equivalent of VisionResults in Vision code
+     Goal is to return a wrapper object containing estimated robot poses and their standard deviations
+     */
+    public QuestResults getQuestResults() {
+        //List<PoseFrame> questResults = new ArrayList<>(); Remove later if code works
+        PoseFrame[] questFrames = null;
+        if (questNav.isConnected() && questNav.isTracking()) {
+            questFrames = questNav.getAllUnreadPoseFrames();
+            //questResults.addAll(questNav.getAllUnreadPoseFrames(), poseEstimator, questNav);  Remove later if code works
+        }
+        return new QuestResults(questFrames);
+    }
+    @Logged(name = "Quest Results")
+    public record QuestResults(PoseFrame[] results){}
+};
+
+
+

src/main/java/frc/robot/subsystems/Swerve.java

@@ -13,8 +13,6 @@
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
-import org.photonvision.EstimatedRobotPose;
-
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.Pigeon2;
@@ -49,7 +47,7 @@
 import frc.robot.Constants.SwerveK;
 import frc.robot.Robot;
 import frc.robot.commands.DriveWheelCharacterization;
-import frc.robot.subsystems.Vision.VisionResults;
+import frc.robot.subsystems.Quest.QuestResults;
 import swervelib.SwerveDrive;
 import swervelib.motors.TalonFXSwerve;
 import swervelib.parser.SwerveParser;
@@ -60,14 +58,14 @@
 public class Swerve extends SubsystemBase { // physicalproperties/conversionFactors/angle/factor = 360.0 deg/4096.0 units per rotation
 
     private final SwerveDrive swerveDrive;
-    private final Supplier<VisionResults> visionSource;
+    private final Supplier<QuestResults> questSource;
     private final TalonFX frontLeft;
     private final TalonFX frontRight;
     private final TalonFX backLeft;
     private final TalonFX backRight;
     private final SysIdRoutine sysIdRoutine; 
     private final PPHolonomicDriveController pathPlannerController = new PPHolonomicDriveController(SwerveK.ppTranslationConstants, SwerveK.ppRotationConstants);
-    private boolean initializedOdometryFromVision = false;
+    private boolean initializedOdometryFromVision = false; //! may not be needed with questnav
     @SuppressWarnings("unused")
     private Pose2d pathPlannerTarget = Pose2d.kZero; // For logging
     // PID Alignment
@@ -80,8 +78,8 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFact
     private final ProfiledPIDController yController = new ProfiledPIDController(SwerveK.translationConstants.kP, SwerveK.translationConstants.kI, SwerveK.translationConstants.kD, SwerveK.defaultTranslationConstraints);
     private final ProfiledPIDController thetaController = new ProfiledPIDController(SwerveK.rotationConstants.kP, SwerveK.rotationConstants.kI, SwerveK.rotationConstants.kD, SwerveK.defaultRotationConstraints);
 
-    public Swerve(Supplier<VisionResults> visionSource, BooleanSupplier overridePathFollowing) {
-        this.visionSource = visionSource;
+    public Swerve(Supplier<QuestResults> questSource, BooleanSupplier overridePathFollowing) {
+        this.questSource = questSource;
         this.overridePathFollowing = overridePathFollowing;
         SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
         SwerveParser parser = null;
@@ -129,15 +127,18 @@ public Swerve(Supplier<VisionResults> visionSource, BooleanSupplier overridePath
     public void periodic() {
         SmartDashboard.putNumber("X setpoint", xController.getSetpoint().position);
         SmartDashboard.putNumber("Y setpoint", yController.getSetpoint().position);
-        for (var result : visionSource.get().results()) {
-            EstimatedRobotPose pose = result.getFirst();
-            if (!initializedOdometryFromVision) {
-                resetOdometry(pose.estimatedPose.toPose2d());
-                initializedOdometryFromVision = true;
-                continue;
-            }
-            swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, result.getSecond());
-        }
+
+    //! this code was origionally made for vision, we altered it for questnav but it may not be needed here
+        // for (var pose : questSource.get().results()) {
+        //     Pose2d questPose = pose.questPose();
+        //     Pose2d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse());
+        //     if (!initializedOdometryFromVision) {
+        //         initializedOdometryFromVision = true;
+        //         continue;
+        //     }
+
+        //     swerveDrive.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS, result.getSecond());
+        // }
     }
 
     private void setupPathPlanner() {
@@ -340,6 +341,7 @@ public String getCurrentCommandName() {
         return cmd.getName();
     }
 
+    //! may not be needed with questnav
     public boolean initializedOdometryFromVision() {
         return initializedOdometryFromVision;
     }

vendordeps/questnavlib.json

@@ -0,0 +1,21 @@
+{
+  "fileName": "questnavlib.json",
+  "name": "questnavlib",
+  "version": "2025-2.1.0-beta",
+  "uuid": "a706fe68-86e5-4aed-92c5-ce05aca007f0",
+  "frcYear": "2025",
+  "mavenUrls": [
+    "https://maven.questnav.gg/releases",
+    "https://maven.questnav.gg/snapshots"
+  ],
+  "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-2.1.0-beta/questnavlib-json-2025-2.1.0-beta.json",
+  "javaDependencies": [
+    {
+      "groupId": "gg.questnav",
+      "artifactId": "questnavlib-java",
+      "version": "2025-2.1.0-beta"
+    }
+  ],
+    "cppDependencies": [],
+    "jniDependencies": []
+}