Helps configure and extend the MARSLib swerve drivetrain subsystem. Use when modifying SwerveDrive, adding odometry sources, tuning PathPlanner, or configuring the PhoenixOdometryThread.
You are a drivetrain engineer for Team MARS 2614. When modifying the swerve subsystem or adding new odometry sources:
The swerve drivetrain is split across 9 files in com.marslib.swerve:
| File | Purpose |
|---|---|
SwerveDrive.java | Top-level subsystem — kinematics, pose estimation, PathPlanner, power shedding |
SwerveModule.java | Per-module wrapper — processes IO inputs, runs SysId, logs per-module telemetry |
SwerveModuleIO.java | IO interface with @AutoLog inputs (drive/turn position, velocity, current) |
SwerveModuleIOTalonFX.java | Real hardware — Phoenix 6 TalonFX + CANcoder |
SwerveModuleIOSim.java | Physics sim — dyn4j-backed wheel slip with Stribeck friction |
GyroIO.java | Gyro interface with @AutoLog inputs (yaw, pitch, roll, rates) |
GyroIOPigeon2.java | Real hardware — CTRE Pigeon2 IMU |
GyroIOSim.java | Physics sim — derives yaw from chassis speeds + Gaussian noise |
PhoenixOdometryThread.java | Background 250Hz thread draining TalonFX signal queues |
// In RobotContainer:
SwerveModule[] modules = new SwerveModule[] {
new SwerveModule(0, new SwerveModuleIOTalonFX(0)), // or IOSim
new SwerveModule(1, new SwerveModuleIOTalonFX(1)),
new SwerveModule(2, new SwerveModuleIOTalonFX(2)),
new SwerveModule(3, new SwerveModuleIOTalonFX(3))
};
SwerveDrive drive = new SwerveDrive(modules, new GyroIOPigeon2(), powerManager);
drive.configurePathPlanner(); // MUST be called separately — not in constructor
AutoBuilder.configure() is NOT called in the SwerveDrive constructor. It lives in the public method configurePathPlanner() which must be called by RobotContainer after construction. This prevents the AutoBuilder static singleton from crashing test classes that construct SwerveDrive.
Module indices 0-3 map to FL, FR, BL, BR. The order is defined in SwerveConstants.MODULE_LOCATIONS. Mismatching indices causes the kinematics to invert, producing diagonal driving. Never reorder without updating both arrays.
PhoenixOdometryThread accumulates position samples at 250Hz into lock-free queues. The SwerveDrive.periodic() method drains these queues and feeds them to the SwerveDrivePoseEstimator. If periodic() is not called (e.g., subsystem not registered), the queues grow unbounded.
SwerveDrive.periodic() polls MARSPowerManager every tick. If voltage drops below CRITICAL_VOLTAGE, stator current limits are dynamically clamped via CAN to prevent brownout. This is non-negotiable — never bypass it.
To add a new odometry source (e.g., VIO SLAM from MARSVision):
swerveDrive.addVisionMeasurement(pose, timestamp, stdDevs) from the vision subsystem's periodic.Constants.VisionConstants — higher std devs = less trust in vision.marslib-vision skill for the vision pipeline details.All drivetrain constants live in frc.robot.SwerveConstants:
MODULE_LOCATIONS — Translation2d array defining module positions relative to robot centerDRIVE_GEAR_RATIO, TURN_GEAR_RATIO — Gear reductionsWHEEL_RADIUS_METERS — Wheel radius for distance calculationMAX_LINEAR_SPEED_MPS — Maximum achievable speedROBOT_MASS_KG, ROBOT_MOI_KG_M2 — For PathPlanner configSwerve/Pose — Estimated Pose2dSwerve/ModuleStates — Current SwerveModuleState[]Swerve/DesiredStates — Commanded SwerveModuleState[]Swerve/ChassisSpeeds — Current ChassisSpeedsSwerve/OdometryPose — Raw odometry (no vision) for comparison