Package frc.alotobots.library.subsystems.vision.questnav

Class QuestNavSubsystem

java.lang.Object

edu.wpi.first.wpilibj2.command.SubsystemBase

frc.alotobots.library.subsystems.vision.questnav.QuestNavSubsystem

All Implemented Interfaces:

Sendable, Subsystem

public class QuestNavSubsystem
extends SubsystemBase

QuestNavSubsystem handles robot localization using a Meta Quest VR headset.

This subsystem leverages the Quest's inside-out SLAM tracking system to provide high-frequency (120Hz) robot pose estimation with sub-centimeter precision. The Quest's visual-inertial odometry provides drift-free position tracking through persistent field mapping.

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
static interface	QuestNavSubsystem.QuestNavConsumer	Functional interface for components that consume QuestNav vision measurements.

Constructor Summary

Constructors

Constructor	Description
QuestNavSubsystem(QuestNavSubsystem.QuestNavConsumer questNavConsumer, QuestNavIO io)	Creates a new QuestNavSubsystem with the given consumer and IO interface.

Method Summary

Modifier and Type	Method	Description
void	periodic()	Periodic update method - called repeatedly by the command scheduler.
void	resetPose(Pose2d pose)	Resets the pose tracking system to a specified position.
void	resetPose(Pose3d pose)	Resets the pose tracking system to a specified position.

Methods inherited from class edu.wpi.first.wpilibj2.command.SubsystemBase

addChild, getName, getSubsystem, initSendable, setName, setSubsystem

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Methods inherited from interface edu.wpi.first.wpilibj2.command.Subsystem

defer, getCurrentCommand, getDefaultCommand, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun

Constructor Details

QuestNavSubsystem

public QuestNavSubsystem(QuestNavSubsystem.QuestNavConsumer questNavConsumer,
 QuestNavIO io)

Creates a new QuestNavSubsystem with the given consumer and IO interface.

Parameters:

questNavConsumer - Consumer that will receive vision measurement updates

io - QuestNavIO object representing the Quest hardware interface

Method Details

periodic

public void periodic()

Periodic update method - called repeatedly by the command scheduler.

This method handles:

• Reading Quest hardware data
• Processing pose observations to estimate robot position
• Filtering out invalid or unreliable detections
• Providing position updates to the QuestNavConsumer
• Monitoring connection, battery, and tracking status
• Logging data for debugging and analysis

resetPose

public void resetPose(Pose2d pose)

Resets the pose tracking system to a specified position.

Parameters:

pose - The new reference pose

resetPose

public void resetPose(Pose3d pose)

Resets the pose tracking system to a specified position.

Parameters:

pose - The new reference pose