Class Swerve

java.lang.Object
com.ctre.phoenix6.swerve.SwerveDrivetrain<TalonFX,TalonFX,CANcoder>
frc.robot.subsystems.swerve.Swerve
All Implemented Interfaces:
AutoCloseable, Subsystem
public class Swerve extends SwerveDrivetrain<TalonFX,TalonFX,CANcoder> implements Subsystem
FM's swerve drivetrain: Phoenix 6's SwerveDrivetrain for control and 250 Hz odometry, with the robot pose owned by PoseFusion on the main loop so it can be replayed.

Where the pose comes from

CTRE's odometry thread still runs every module at 250 Hz. Its telemetry callback, on that thread, only copies each sample into a bounded queue. Each robot loop periodic() drains the queue into SwerveInputs, records it with AdvantageKit, and hands every sample to PoseFusion, which is the pose everything on the robot reads (getRobotPose()). Vision measurements go to PoseFusion too (see Vision). CTRE's internal estimate is kept aligned with the fused pose (syncCtrePose()) because its field-centric requests rotate by its own heading.

From the 2026 offseason branch

Per-module current and connection logging at 10 Hz, the SwerveAlignment encoder publisher, skipping the bus optimisation on a dead bus (CanConfigBudget), and never doing log work on CTRE's odometry thread. FM's own drive modes (pilot aim at target through ShotCalculator) are kept from main; the offseason bot's turret-pivot mode is not.

Simulation

maple-sim has no WPILib 2027 build, so the sim is CTRE's own updateSimState. To give the vision comparison something to do, the sim keeps a separate perfect truth odometry and feeds the robot code a version with wheel-radius and gyro-drift error (Swerve.SimErrors).

  • Field Details

    • TRANSLATION_ERROR_MARGIN_METERS

      public static final double TRANSLATION_ERROR_MARGIN_METERS

    • DRIVE_TO_POINT_STATIC_FRICTION_CONSTANT

      public static final double DRIVE_TO_POINT_STATIC_FRICTION_CONSTANT
      See Also:

  • Constructor Details

    • Swerve

      public Swerve(SwerveConfig config)
      Constructs a new Swerve drive subsystem.
      Parameters:
      config - drivetrain constants and module configurations

  • Method Details

    • isTiltScaledOdometry

      public boolean isTiltScaledOdometry()
      Whether the dashboard switch for tilt-scaled odometry is on (its default).

    • getMountCheck

      public ImuMountCheck getMountCheck()
      The gyro mount check, fed every loop, for the pre-match checks.

    • getTilt

      public TiltState getTilt()
      The Pigeon's tilt state this loop.

    • isTilted

      public boolean isTilted()
      Whether the robot is tilted more than TiltState.TILTED_DEG, e.g. on a bump.

    • wasBumpedRecently

      public boolean wasBumpedRecently()
      Whether the robot took a knock (collision, bump landing) in the last half second.

    • updateInputs

      public void updateInputs()
      Reads the drivetrain into inputs, records them, and integrates this loop's odometry into the fused pose. Called by Robot at the top of every loop, before vision and before the scheduler, so everything this loop sees the same pose.

    • moduleRebootedThisLoop

      public boolean moduleRebootedThisLoop(int module)
      Whether a module's drive motor rebooted this loop, not counting Phoenix's report of the first contact after boot. Use this, not the raw ModuleResetOccurred input.
      Parameters:
      module - module index, in SwerveAlignment.MODULE_NAMES order
      Returns:
      true on the loop the reboot is seen

    • gyroRebootedThisLoop

      public boolean gyroRebootedThisLoop()
      Whether the Pigeon rebooted this loop, not counting Phoenix's report of the first contact after boot. Use this, not the raw GyroResetOccurred input.
      Returns:
      true on the loop the reboot is seen

    • periodic

      public void periodic()
      Specified by:
      periodic in interface Subsystem

    • afterVision

      public void afterVision()
      Called by Robot after vision has been fused, so CTRE is re-aligned with the pose this loop's measurements produced.

    • logBatteryUsage

      protected void logBatteryUsage()
      Reports drive and steer supply current to the battery logger every loop and logs per-module and summed currents at 10 Hz. Diagnostic outputs only; nothing decides on them, so they are read straight from the signals rather than through SwerveInputs.

    • getCurrentCommandName

      protected String getCurrentCommandName()

    • getRobotPose

      public Pose2d getRobotPose()
      The robot's fused pose: odometry plus every enabled vision source, integrated on the main loop from logged inputs.
      Returns:
      the robot pose on the field

    • getPoseAtTimestamp

      public Pose2d getPoseAtTimestamp(double timestampSeconds)
      Get the robot's pose at a specific timestamp using interpolation.
      Parameters:
      timestampSeconds - the timestamp to sample at, Timer.getTimestamp() base
      Returns:
      the interpolated pose, or current pose if the timestamp is not in the buffer

    • resetPose

      public void resetPose(Pose2d pose)
      Resets every pose track (and, in sim, the simulated robot) to pose.
      Overrides:
      resetPose in class SwerveDrivetrain<TalonFX,TalonFX,CANcoder>

    • inXzone

      public Trigger inXzone(double minXmeter, double maxXmeter)

    • inYzone

      public Trigger inYzone(double minYmeter, double maxYmeter)

    • inXzoneAlliance

      public Trigger inXzoneAlliance(double minXmeter, double maxXmeter)
      Whether the robot is in an X band of the field, flipped for red.
      Parameters:
      minXmeter - the minimum X coordinate in meters
      maxXmeter - the maximum X coordinate in meters
      Returns:
      the Trigger

    • inYzoneAlliance

      public Trigger inYzoneAlliance(double minYmeter, double maxYmeter)
      Whether the robot is in a Y band of the field, flipped for red.
      Parameters:
      minYmeter - the minimum Y coordinate in meters
      maxYmeter - the maximum Y coordinate in meters
      Returns:
      the Trigger

    • isInNeutralZone

      public boolean isInNeutralZone()
      Returns true when the robot is inside the neutral zone.

    • isInEnemyAllianceZone

      public boolean isInEnemyAllianceZone()
      Returns true when the robot is inside the opposing alliance's zone (pose X is flipped for red so the same rectangle works for both alliances).

    • inNeutralZone

      public Trigger inNeutralZone()

    • inEnemyAllianceZone

      public Trigger inEnemyAllianceZone()

    • inFieldRight

      public Trigger inFieldRight()

    • inFieldLeft

      public Trigger inFieldLeft()

    • isGoingTooFast

      public boolean isGoingTooFast(double thresholdSpeed)
      Returns true if the robot is moving faster than the threshold.
      Parameters:
      thresholdSpeed - the speed threshold in meters per second
      Returns:
      true if the current linear speed exceeds the threshold

    • overSpeedTrigger

      public Trigger overSpeedTrigger(double thresholdSpeed)

    • getCurrentRobotChassisSpeeds

      public ChassisVelocities getCurrentRobotChassisSpeeds()
      Robot-relative chassis velocity measured from the modules this loop (a logged input).
      Returns:
      the current robot chassis velocity

    • getYawRateRadPerSec

      public double getYawRateRadPerSec()
      Gyro yaw rate this loop, rad/s (a logged input).

    • reorient

      protected Command reorient(double angleDegrees)

    • reorientForward

      public Command reorientForward()

    • reorientLeft

      public Command reorientLeft()

    • reorientBack

      public Command reorientBack()

    • reorientRight

      public Command reorientRight()

    • getClosestCardinal

      protected double getClosestCardinal()

    • cardinalReorient

      protected Command cardinalReorient()

    • frontClosestToAngle

      public boolean frontClosestToAngle(double angleDegrees)

    • getRotationDifference

      public double getRotationDifference(double angle1, double angle2)
      Shortest absolute difference between two angles, degrees (0-180).

    • setWantedState

      public void setWantedState(Swerve.WantedState state)

    • getAimHeadingErrorRadians

      public double getAimHeadingErrorRadians()
      Fused heading minus the heading the aim request is holding, wrapped to (-pi, pi]. Read from the fused pose, so it replays.
      Returns:
      the heading error in radians, or NaN when the drivetrain is not aiming

    • isAiming

      public boolean isAiming()
      Whether the drivetrain is holding a heading on the shot solution this loop.

    • isAtDesiredRotation

      public boolean isAtDesiredRotation()

    • isAtDesiredRotation

      public boolean isAtDesiredRotation(double toleranceRadians)
      Whether the robot heading is within tolerance of the aim target. Computed from the fused pose rather than CTRE's heading controller so it replays.
      Parameters:
      toleranceRadians - the allowed heading error in radians
      Returns:
      true while aiming and inside the tolerance

    • getSimTruthPose

      public Optional<Pose2d> getSimTruthPose()
      Truth pose in simulation, empty elsewhere.

    • getTeleopVelocityCoefficient

      public double getTeleopVelocityCoefficient()

    • setTeleopVelocityCoefficient

      public void setTeleopVelocityCoefficient(double teleopVelocityCoefficient)

    • getRotationVelocityCoefficient

      public double getRotationVelocityCoefficient()

    • setRotationVelocityCoefficient

      public void setRotationVelocityCoefficient(double rotationVelocityCoefficient)

    • getConfig

      public SwerveConfig getConfig()

    • getInputs

      public SwerveInputs getInputs()

    • getPoseFusion

      public PoseFusion getPoseFusion()
      The robot pose and every per-source comparison track.