Quest offsets and vendordep

Author: blaze-developer

src/main/java/frc/robot/RobotContainer.java

@@ -166,7 +166,7 @@ private RobotContainer() {
                 case COMPETITION:
 
                     drivebase_ = new Drive(
-                        new GyroIOQuest(),
+                        new GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus),
                         ModuleIOTalonFX::new,
                         CompTunerConstants.FrontLeft,
                         CompTunerConstants.FrontRight,
@@ -182,7 +182,6 @@ private RobotContainer() {
                         new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation)
                     );
 
-
                     try {
                         manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware());
                     } catch (Exception ex) {

src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java

@@ -3,7 +3,7 @@
 import java.util.Queue;
 
 import edu.wpi.first.math.geometry.Rotation2d;
-import frc.robot.subsystems.vision.QuestNav;
+import gg.questnav.questnav.QuestNav; 
 
 public class GyroIOQuest implements GyroIO {
 
@@ -19,7 +19,7 @@ public GyroIOQuest() {
 
     @Override
     public void updateInputs(GyroIOInputs inputs) {
-        inputs.connected = quest_.getConnected();
+        inputs.connected = quest_.isConnected();
         inputs.yawPosition = getYaw();
 
         inputs.odometryYawPositions = yawPositionQueue_

src/main/java/frc/robot/subsystems/vision/QuestNav.java

@@ -3,8 +3,6 @@
 import org.littletonrobotics.junction.AutoLog;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Quaternion;
 
 public interface TrackerIO {
     @AutoLog
@@ -17,8 +15,6 @@ public static class TrackerInputs {
         public long trackingLostCount = 0;
 
         public Pose2d pose = new Pose2d();
-        public Pose3d pose3d = new Pose3d();
-        public Quaternion quaternion = new Quaternion();
     }
 
     public default void updateInputs(TrackerInputs inputs) {}

src/main/java/frc/robot/subsystems/vision/TrackerIO.java

@@ -1,32 +1,41 @@
 package frc.robot.subsystems.vision;
 
+import static edu.wpi.first.units.Units.Inches;
+
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import gg.questnav.questnav.QuestNav;
 
 public class TrackerIOQuest implements TrackerIO {
+    
+    private static final Transform2d robotToQuest = new Transform2d(
+        Inches.of(12),
+        Inches.of(9.25),
+        Rotation2d.kZero
+    );
 
     private final QuestNav quest = new QuestNav();
 
     @Override
     public void updateInputs(TrackerInputs inputs) {
 
-        quest.cleanupResponses();
-        quest.processHeartbeat();
+        quest.commandPeriodic();
 
-        inputs.connected = quest.getConnected();
-        inputs.timestamp = quest.getTimestamp();
         inputs.batteryPercent = quest.getBatteryPercent();
+        inputs.connected = quest.isConnected();
+        inputs.isTracking = quest.isTracking();
+
         inputs.frameCount = quest.getFrameCount();
-        inputs.isTracking = quest.getTrackingStatus();
         inputs.trackingLostCount = quest.getTrackingLostCounter();
-
-        inputs.pose = quest.getPose();
-        inputs.pose3d = quest.getPose3d();
-        inputs.quaternion = quest.getQuaternion();
+        
+        inputs.timestamp = quest.getDataTimestamp();
+        inputs.pose = quest.getPose().transformBy(robotToQuest.inverse());
     }
 
     @Override
     public void setPose(Pose2d pose) {
-        quest.setPose(pose);
+        quest.setPose(pose.transformBy(robotToQuest));
     }
 
 }

src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java

@@ -0,0 +1,21 @@
+{
+    "fileName": "questnavlib.json",
+    "name": "questnavlib",
+    "version": "2025-1.0.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-1.0.0-beta/questnavlib-json-2025-1.0.0-beta.json",
+    "javaDependencies": [
+        {
+            "groupId": "gg.questnav",
+            "artifactId": "questnavlib-java",
+            "version": "2025-1.0.0-beta"
+        }
+    ],
+    "cppDependencies": [],
+    "jniDependencies": []
+}