Package frc.alotobots.library.subsystems.vision.oculus

Class OculusSubsystem

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem
All Implemented Interfaces:
Sendable, Subsystem, PoseSource
public class OculusSubsystem extends SubsystemBase implements PoseSource
Manages communication and pose estimation with a Meta Quest VR headset.

This subsystem leverages the Quest's inside-out SLAM tracking system to provide high-frequency (120Hz) robot pose estimation. Key features:

- Global SLAM-based localization - Field mapping and persistence - Sub-millimeter tracking precision - High update rate (120Hz) - Drift-free position tracking - Fast relocalization

The system operates in phases: 1. Pre-match mapping to capture field features 2. Initial pose acquisition and alignment 3. Continuous pose updates during match 4. Recovery handling if tracking is lost

  • Constructor Details

    • OculusSubsystem

      public OculusSubsystem(OculusIO io)
      Creates a new OculusSubsystem.

      Initializes communication with Quest hardware and prepares logging systems. The subsystem starts in an uninitialized state requiring pose calibration.

      Parameters:
      io - Interface for Quest hardware communication

  • Method Details

    • periodic

      public void periodic()
      Updates subsystem state and processes Quest data.

      Called periodically by the command scheduler. This method: - Updates hardware inputs - Processes new pose data - Handles state transitions - Manages reset operations - Updates logging

      Specified by:
      periodic in interface Subsystem

    • resetToPose

      public boolean resetToPose(Pose2d targetPose)
      Initiates pose reset operation.

      Attempts to reset Quest SLAM origin to align with target pose.

      Parameters:
      targetPose - Desired robot pose after reset
      Returns:
      true if reset initiated successfully

    • zeroHeading

      public boolean zeroHeading()
      Initiates heading reset operation.

      Attempts to zero current Quest heading measurement.

      Returns:
      true if reset initiated successfully

    • ping

      public boolean ping()
      Tests Quest communication.

      Sends ping command to verify hardware connectivity.

      Returns:
      true if ping initiated successfully

    • isConnected

      public boolean isConnected()
      Checks if the pose source is currently connected and providing valid data.

      This method should: - Verify hardware connectivity (if applicable) - Check data freshness - Validate sensor readings - Monitor update frequency

      Quest connection is determined by checking if we've received any new Quest timestamp updates within our timeout window. Small variations in update timing are allowed, only triggering disconnect on significant delays.

      Specified by:
      isConnected in interface PoseSource
      Returns:
      true if the source is connected and providing valid data

    • getCurrentPose

      public Pose2d getCurrentPose()
      Gets the most recent pose estimate from this source.

      The returned pose should be: - In field-relative coordinates - Using the standard FRC coordinate system: - Origin at field corner - +X towards opposite alliance wall - +Y towards driver station - CCW positive rotation

      If no valid pose is available, returns null.

      Returns field-relative robot pose from Quest SLAM.

      Specified by:
      getCurrentPose in interface PoseSource
      Returns:
      The current robot pose estimate, or null if unavailable

    • getStdDevs

      public Matrix<N3,N1> getStdDevs()
      Gets the standard deviations of measurement uncertainty for this pose source.

      Returns a 3x1 matrix containing standard deviations for: - X position (meters) - Y position (meters) - Rotation (radians)

      These values should: - Reflect real measurement uncertainty - Scale with distance/conditions - Account for systematic errors - Consider environmental factors

      Returns constant measurement uncertainty for Quest tracking.

      Specified by:
      getStdDevs in interface PoseSource
      Returns:
      3x1 matrix of standard deviations [x, y, theta]

    • getSourceName

      public String getSourceName()
      Gets a human-readable identifier for this pose source.

      Used for: - Logging - Debugging - User interfaces - Status reporting

      Specified by:
      getSourceName in interface PoseSource
      Returns:
      String identifier for this pose source