Package com.cdarobotics.cdalib.subsystems

Class ThreadedIMU

java.lang.Object
com.cdarobotics.cdalib.subsystems.Subsystem
com.cdarobotics.cdalib.subsystems.ThreadedIMU
public class ThreadedIMU extends Subsystem
A Subsystem that reads the IMU on its own background thread so the main loop never blocks on it.

The universal IMU is an I2C device, and I2C is not covered by Lynx bulk caching — so a normal getRobotYawPitchRollAngles() in the loop costs a full blocking transaction (several milliseconds) every cycle. This subsystem instead spins a reader thread that polls the IMU continuously and publishes each reading to a cached snapshot; the getters return that snapshot in constant time, removing IMU latency from every loop.

Register it with registerSubsystem(...) in your preload() — ModularOpMode then starts the reader thread on start() and stops it on OpMode end. The IMU must already be initialized (via imu.initialize(...) with your hub orientation) before this subsystem is registered; like IMUDevice it does not initialize the IMU itself.

All hardware access to the IMU (the periodic read and resetYaw()) is serialized on an internal lock, so it is safe to call resetYaw() from the loop thread while the reader thread is running. Getters return 0 until the first reading lands.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    static final long
    DEFAULT_POLL_INTERVAL_MS
    Default gap between reads (~200 Hz), far faster than any loop, without pegging a core.

  • Constructor Summary

    Constructors
    Constructor
    Description
    ThreadedIMU(com.qualcomm.robotcore.hardware.HardwareMap hardwareMap, String name)
    Resolves the IMU from the hardware map by configured name.
    ThreadedIMU(com.qualcomm.robotcore.hardware.HardwareMap hardwareMap, String name, long pollIntervalMs)
    Resolves the IMU from the hardware map by name, with a custom poll interval.
    ThreadedIMU(com.qualcomm.robotcore.hardware.IMU imu)
    Wraps an already-initialized IMU, polling at the default rate.
    ThreadedIMU(com.qualcomm.robotcore.hardware.IMU imu, long pollIntervalMs)
    Wraps an already-initialized IMU with a custom poll interval.

  • Method Summary

    Modifier and Type
    Method
    Description
    com.qualcomm.robotcore.hardware.IMU
    getImu()
     
    double
    getPitch(org.firstinspires.ftc.robotcore.external.navigation.AngleUnit unit)
     
    double
    getRoll(org.firstinspires.ftc.robotcore.external.navigation.AngleUnit unit)
     
    double
    getYaw(org.firstinspires.ftc.robotcore.external.navigation.AngleUnit unit)
     
    void
    init()
    Called once when the OpMode initializes.
    void
    init_loop()
    Called repeatedly during the OpMode init phase, after Subsystem.init() and before Subsystem.start(), while the driver is still on the init screen.
    void
    resetYaw()
    Resets the yaw so the current heading reads as zero.
    void
    start()
    Launches the reader thread.
    void
    stop()
    Signals the reader thread to stop and waits briefly for it to finish.
    void
    update()
    Called every loop.

    Methods inherited from class java.lang.Object

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

  • Field Details

    • DEFAULT_POLL_INTERVAL_MS

      public static final long DEFAULT_POLL_INTERVAL_MS
      Default gap between reads (~200 Hz), far faster than any loop, without pegging a core.
      See Also:

  • Constructor Details

    • ThreadedIMU

      public ThreadedIMU(com.qualcomm.robotcore.hardware.IMU imu)
      Wraps an already-initialized IMU, polling at the default rate.

    • ThreadedIMU

      public ThreadedIMU(com.qualcomm.robotcore.hardware.IMU imu, long pollIntervalMs)
      Wraps an already-initialized IMU with a custom poll interval.
      Parameters:
      pollIntervalMs - milliseconds the reader sleeps between reads; 0 polls as fast as possible (busy-spins a core — prefer a small non-zero value).

    • ThreadedIMU

      public ThreadedIMU(com.qualcomm.robotcore.hardware.HardwareMap hardwareMap, String name)
      Resolves the IMU from the hardware map by configured name. It must still be initialized separately.

    • ThreadedIMU

      public ThreadedIMU(com.qualcomm.robotcore.hardware.HardwareMap hardwareMap, String name, long pollIntervalMs)
      Resolves the IMU from the hardware map by name, with a custom poll interval.

  • Method Details

    • init

      public void init()
      Description copied from class: Subsystem
      Called once when the OpMode initializes. Grab hardware and set starting state here.
      Specified by:
      init in class Subsystem

    • init_loop

      public void init_loop()
      Description copied from class: Subsystem
      Called repeatedly during the OpMode init phase, after Subsystem.init() and before Subsystem.start(), while the driver is still on the init screen. Use it for work that must keep running until play is pressed, such as reporting sensor readings or waiting on hardware.
      Specified by:
      init_loop in class Subsystem

    • start

      public void start()
      Launches the reader thread. Idempotent — calling twice while running does nothing.
      Specified by:
      start in class Subsystem

    • update

      public void update()
      Description copied from class: Subsystem
      Called every loop. Read inputs and drive the mechanism here.
      Specified by:
      update in class Subsystem

    • stop

      public void stop()
      Signals the reader thread to stop and waits briefly for it to finish.
      Specified by:
      stop in class Subsystem

    • getYaw

      public double getYaw(org.firstinspires.ftc.robotcore.external.navigation.AngleUnit unit)
      Returns:
      the most recent yaw (heading) in the requested unit, or 0 before the first read.

    • getPitch

      public double getPitch(org.firstinspires.ftc.robotcore.external.navigation.AngleUnit unit)
      Returns:
      the most recent pitch in the requested unit, or 0 before the first read.

    • getRoll

      public double getRoll(org.firstinspires.ftc.robotcore.external.navigation.AngleUnit unit)
      Returns:
      the most recent roll in the requested unit, or 0 before the first read.

    • resetYaw

      public void resetYaw()
      Resets the yaw so the current heading reads as zero. Safe to call from the loop thread — it is serialized against the reader thread's IMU access.

    • getImu

      public com.qualcomm.robotcore.hardware.IMU getImu()
      Returns:
      the wrapped SDK IMU. Direct calls from the loop thread race with the reader thread; synchronize on nothing exposed here — prefer the getters and resetYaw().