Unit 7 · Lesson 6

The SwerveDrivetrain Subsystem

The four SwerveModule objects exist. The kinematics and odometry infrastructure exists. Now we build the conductor — the top-level SwerveDrivetrain subsystem that wires everything together, presents a clean public API to the rest of the codebase, and is what RobotContainer and every drive command actually talk to.

By the end of this lesson, you will:

Structure the complete SwerveDrivetrain subsystem with all required fields and a clean constructor
Write a periodic() method that updates odometry, Field2d, and per-module telemetry every loop
Write the drive(ChassisSpeeds) method as the single entry point for all chassis motion commands
Expose a minimal, purposeful public API: getPose(), resetPose(), getHeading(), stop()
Register the subsystem with PathPlanner's auto builder so path-following works out of the box
Explain why SwerveDrivetrain is a subsystem and SwerveModule is not

Architecture: The Conductor Pattern

The relationship between SwerveDrivetrain and SwerveModule mirrors how an orchestra conductor relates to individual musicians. The conductor (SwerveDrivetrain) receives the score (ChassisSpeeds), breaks it into four parts (module states via kinematics), and hands each part to the right musician (SwerveModule). Each musician executes their part independently. The conductor never reaches into a violin to move the bow — that would break the encapsulation. It just communicates through the agreed interface.

The Complete Subsystem — Tabbed Explorer

The SwerveDrivetrain has four sections. Click each tab to explore the code and click the highlighted tokens for explanations.

SwerveDrivetrain.java

public class SwerveDrivetrain extends SubsystemBase {

  // ── Four modules ─────────────────────────────────
  private final SwerveModule m_fl, m_fr, m_bl, m_br;
  private final Pigeon2 m_pigeon = new Pigeon2(Constants.Swerve.kPigeonId);

  // ── Kinematics ───────────────────────────────────
  private final SwerveDriveKinematics m_kinematics =
    new SwerveDriveKinematics(
      new Translation2d( kHalf, kHalf), // FL
      new Translation2d( kHalf, -kHalf), // FR
      new Translation2d(-kHalf, kHalf), // BL
      new Translation2d(-kHalf, -kHalf) // BR
    );
  private static final double kHalf =
    Constants.Swerve.kTrackWidthMeters / 2.0;

  // ── Odometry + visualization ─────────────────────
  private final SwerveDriveOdometry m_odometry;
  private final Field2d m_field = new Field2d();

  public SwerveDrivetrain() {
    // Build four modules from Constants
    C c = Constants.Swerve;
    m_fl = new SwerveModule(c.kFLDrive, c.kFLSteer, c.kFLEncoder, c.kFLOffset, true);
    m_fr = new SwerveModule(c.kFRDrive, c.kFRSteer, c.kFREncoder, c.kFROffset, false);
    m_bl = new SwerveModule(c.kBLDrive, c.kBLSteer, c.kBLEncoder, c.kBLOffset, true);
    m_br = new SwerveModule(c.kBRDrive, c.kBRSteer, c.kBREncoder, c.kBROffset, false);

    m_odometry = new SwerveDriveOdometry(
      m_kinematics, getHeading(), getModulePositions(), new Pose2d());

    SmartDashboard.putData("Field", m_field);
    configurePathPlanner();
  }

  private void configurePathPlanner() {
    AutoBuilder.configure(
      this::getPose,                   // pose supplier
      this::resetPose,                 // pose consumer
      this::getChassisSpeeds,          // robot-relative speeds
      (speeds, ff) -> drive(speeds), // drive consumer
      new PPHolonomicDriveController(...),
      new RobotConfig(...),            // robot physical config
      () -> isOnRedAlliance(),        // alliance color flip
      this                            // subsystem requirement
    );
  }
}

← click a highlighted token

@Override
public void periodic() {
  // 1. Update pose estimate from encoders + gyro
  m_odometry.update(getHeading(), getModulePositions());

  // 2. Push updated pose to Field2d widget
  m_field.setRobotPose(m_odometry.getPoseMeters());

  // 3. Per-module telemetry — uses name prefix
  m_fl.updateTelemetry("FL");
  m_fr.updateTelemetry("FR");
  m_bl.updateTelemetry("BL");
  m_br.updateTelemetry("BR");

  // 4. Drivetrain-level telemetry
  SmartDashboard.putNumber("Drive/HeadingDeg",
    getHeading().getDegrees());
  SmartDashboard.putString("Drive/Pose",
    getPose().toString());
  SmartDashboard.putNumber("Drive/BatteryV",
    RobotController.getBatteryVoltage());
}

// Helper: build SwerveModulePosition array
private SwerveModulePosition[] getModulePositions() {
  return new SwerveModulePosition[] {
    m_fl.getPosition(), m_fr.getPosition(),
    m_bl.getPosition(), m_br.getPosition()
  };
}

// Helper: build SwerveModuleState array for fwd kinematics
public ChassisSpeeds getChassisSpeeds() {
  return m_kinematics.toChassisSpeeds(
    m_fl.getState(), m_fr.getState(),
    m_bl.getState(), m_br.getState()
  );
}

← click a highlighted token

/** The single entry point for all chassis motion. */
public void drive(ChassisSpeeds speeds) {
  // Discretize removes "rotation drift" at low speed
  ChassisSpeeds discrete = ChassisSpeeds.discretize(speeds, 0.02);

  // Inverse kinematics → 4 module states
  SwerveModuleState[] states =
    m_kinematics.toSwerveModuleStates(discrete);

  // Normalize: no module exceeds max speed
  SwerveDriveKinematics.desaturateWheelSpeeds(
    states, Constants.Swerve.kMaxSpeedMetersPerSecond);

  // Command each module
  m_fl.setDesiredState(states[0]);
  m_fr.setDesiredState(states[1]);
  m_bl.setDesiredState(states[2]);
  m_br.setDesiredState(states[3]);
}

/** Stop all modules immediately (E-stop / disabled). */
public void stop() {
  drive(new ChassisSpeeds());
}

/** Lock wheels in X pattern to resist pushing. */
public void lockWheels() {
  m_fl.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees( 45)));
  m_fr.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
  m_bl.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
  m_br.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees( 45)));
}

← click a highlighted token

// ── Pose API ─────────────────────────────────────────
public Pose2d getPose() {
  return m_odometry.getPoseMeters();
}

public void resetPose(Pose2d pose) {
  m_odometry.resetPosition(
    getHeading(), getModulePositions(), pose);
}

// ── Gyro API ─────────────────────────────────────────
public Rotation2d getHeading() {
  return Rotation2d.fromDegrees(
    -m_pigeon.getYaw().getValueAsDouble());
}

public void zeroHeading() {
  m_pigeon.setYaw(0.0);
}

// ── Kinematics accessor ───────────────────────────────
public SwerveDriveKinematics getKinematics() {
  return m_kinematics;
}

← click a highlighted token

💡 The lockWheels() X-pattern: competition defense secret

Pointing the four wheels at 45°, −45°, −45°, 45° (forming an X) creates interlocking resistance to being pushed from any direction. When an opponent tries to push you during endgame or a defensive stand, the X-pattern means no single push direction can overcome all four wheels simultaneously. Bind this to a button held by the driver during endgame. The steering motors will hold the pattern actively — it's not a passive brake, it's active resistance.

🔍 LRI Perspective: "One drive() method. Nothing else commands the motors."

The discipline of having a single drive(ChassisSpeeds) method as the only path to hardware commands pays enormous dividends in debugging. When something drives wrong, there is exactly one place to look: what was passed to drive()? Add a single SmartDashboard publish of the incoming ChassisSpeeds at the top of drive(), and you can instantly see what speeds the command is requesting versus what the robot is actually doing. If the command is sending the right speeds and the robot behaves wrong, it's the module. If the command is sending wrong speeds, it's the input pipeline. One log line. Two possible suspects. That's a ten-minute debug at competition, not a two-hour one.

The Public API Surface

The table below documents every public method — the deliberate, minimal surface that commands, autonomous routines, and RobotContainer interact with. Nothing else should be public.

Method	Returns	Who calls it
drive(ChassisSpeeds)	void	Drive commands, PathPlanner, auto commands
stop()	void	Default command fallback, disabled init
lockWheels()	void	Endgame / defense command binding
getPose()	Pose2d	PathPlanner, vision commands, Dashboard
resetPose(Pose2d)	void	autonomousInit(), PathPlanner
getHeading()	Rotation2d	Field-oriented drive command (Lesson 9)
zeroHeading()	void	Start-button binding in RobotContainer
getChassisSpeeds()	ChassisSpeeds	PathPlanner (robot-relative speed feedback)
getKinematics()	SwerveDriveKinematics	PathPlanner RobotConfig (rarely needed)

🔌 System Check — Drivetrain Subsystem Verification

After building the subsystem, verify in order:

Deploy and enable. In SmartDashboard, confirm Drive/HeadingDeg updates when you rotate the robot by hand. Confirm Drive/Pose shows a valid Pose2d string (not null). Confirm the Field2d widget appears in Glass or Shuffleboard.
Call drive(new ChassisSpeeds(0.5, 0, 0)) from a test trigger. All four wheels should point forward and spin. SmartDashboard's per-module telemetry should show ~0° steer angle and a positive velocity on all four modules.
Call lockWheels() from a button. Confirm the four wheels rotate to the X-pattern. Physically try to push the robot — it should resist. This validates both the steering PID is working and the stop command is operational.
Confirm getChassisSpeeds() returns approximately the same speeds you passed into drive(). Large discrepancies indicate CANcoder offset errors or encoder reading problems.

Knowledge Check

1. A teammate adds a method public void spinFrontLeft(double speed) to SwerveDrivetrain that directly calls m_fl.m_driveMotor.set(speed). What is wrong with this approach?

A The method name doesn't follow Java naming conventions.
B It bypasses the single drive(ChassisSpeeds) entry point, breaking the architectural contract. Direct motor access circumvents kinematics, desaturation, and odometry state. It also violates SwerveModule's encapsulation by accessing its private field from outside the class.
C It will cause a CAN bus conflict with the normal drive loop.
D Subsystems cannot hold references to non-subsystem objects like SwerveModule.

2. ChassisSpeeds.discretize(speeds, 0.02) is called at the top of drive(). Why is this important when combining translation and rotation at low speed?

A It prevents the robot from exceeding maximum speed by capping all values.
B Swerve kinematics assumes continuous motion, but the robot only updates every 20 ms. At slow combined translation+rotation, this discretization error causes the robot to drift in an arc instead of a straight line. discretize() corrects for the 20 ms sample period, making the robot's actual path match the intended path at low speed.
C It converts ChassisSpeeds from field-relative to robot-relative coordinates.
D It rounds speeds to the nearest 0.1 m/s to reduce CAN bus traffic.

3. The stop() method calls drive(new ChassisSpeeds()) rather than calling m_fl.stopMotor() directly on each module. What advantage does this pattern provide?

A It's shorter to type and saves three lines of code.
B Every stop passes through the same code path as normal driving — kinematics, desaturation, and the per-module near-zero guard all apply. The modules' motors stop cleanly via their own setDesiredState() logic. If you ever add logging or safety checks to drive(), they automatically apply to stops as well.
C Calling stopMotor() directly would cause the modules to enter coast mode, losing the brake position hold.
D stopMotor() is not available on TalonFX in Phoenix 6.

💪 Practice Prompt

Build the Complete Drivetrain Subsystem

Create SwerveDrivetrain.java extending SubsystemBase. Add all four SwerveModule fields, the Pigeon2, kinematics, odometry, and Field2d. Construct the modules in the constructor using your Constants.Swerve values. Verify the project compiles with no errors before proceeding.
Implement periodic() with odometry update, Field2d update, per-module telemetry calls, heading publishing, and battery voltage publishing. Open Glass or Shuffleboard and confirm every channel appears with valid values after deploying.
Implement drive(ChassisSpeeds) with ChassisSpeeds.discretize(), toSwerveModuleStates(), desaturateWheelSpeeds(), and the four setDesiredState() calls. Add SmartDashboard.putNumber("Drive/VxCmd", speeds.vxMetersPerSecond) at the top of the method. Test with drive(new ChassisSpeeds(0.5, 0, 0)) from a test trigger.
Implement lockWheels() and bind it to the Start button in RobotContainer using .onTrue(). Test it physically — the robot should resist being pushed from any direction.
Stretch goal: Add the configurePathPlanner() method. Install the PathPlanner vendor library if not already present. Provide stub implementations for the RobotConfig and PPHolonomicDriveController using placeholder values — PathPlanner tuning comes in Unit 9. Confirm the project compiles with PathPlanner integrated.