Skip to content
MARSLib Documentation

High-Frequency Swerve

A standard FRC robot executes its control loop every 20ms (50Hz). This is fine for moving an elevator, but it is unacceptably slow for Swerve Odometry. If a robot moving at 5 m/s only calculates its position 50 times a second, the math inherently creates a “drift” of several centimeters per second.

MARSLib solves this by utilizing the PhoenixOdometryThread.

1. The 250Hz Thread

Instead of relying on the main RoboRIO thread, our swerve architecture spins up an isolated background thread that communicates directly with the CTRE CANivore bus. This thread calculates the robot’s physical kinematics at 250Hz.

2. MegaTag 2.0 Integration

Limelight 4’s MegaTag 2.0 system relies heavily on the IMU Yaw rates to compensate for camera latency. Because we process the Pigeon 2.0 data at 250Hz, our gyro-seeding to the Limelight is incredibly precise, allowing for flawless localization even while spinning and traversing the Bump.

// Asynchronously injecting vision without locking the thread
public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix<N3, N1> stdDevs) {
  poseEstimator.addVisionMeasurement(visionPose, timestamp, stdDevs);
}

3. 2D Kinematic Traction Control

Standard WPILib implementations apply independent SlewRateLimiters to the X and Y joystick axes. However, if the driver pushes diagonally, the robot attempts to accelerate at 1.41x the limit, instantly breaking carpet friction.

To solve this, MARSLib uses a unified TractionControlLimiter that calculates the magnitude of the requested 2D acceleration vector and strictly caps it below the kinetic slipping envelope of the FRC carpet (approx 1.1g, or 10.78 m/s²). This guarantees the swerve drive never breaks static friction natively, preserving your odometry perfectly!



📖 Further Reading & External Resources