Skip to content

Lesson 6: Drivetrain

Jump to bottom
kevinzhang edited this page Jun 22, 2026 · 1 revision

Lesson 04: Drivetrain Introduction

Purpose

Understand the current drivetrain architecture well enough to trace driver input, assisted drive modes, autonomous path requests, CTRE swerve requests, simulation, and drivetrain logs.

Drivetrain code is different from a simple motor subsystem. It mixes driver feel, field coordinates, robot pose, vendor swerve code, autonomous path following, simulation, and hardware configuration. The goal of this lesson is not to memorize every CTRE class. The goal is to learn the layers and know where to inspect behavior before editing.

Prerequisites

  • Finish Lessons 1-3.
  • Read the software overview, especially the Driver Station, roboRIO, CAN, NetworkTables, and logging sections.
  • Understand the basic Lesson 3 pattern: high-level robot behavior should not directly own every hardware detail.
  • Have 971-second-robot-2026 cloned locally.

Time Box

1-2 hours

Learning Goals

By the end of this lesson, you should be able to:

  • Explain the role of the drivetrain in the robot.
  • Trace manual drive from controller input to CTRE SwerveRequest.
  • Explain the responsibilities of Drive, Manual, ThetaLock, AutoAlign, and CommandSwerveDrivetrain.
  • Understand how autonomous path following writes drivetrain requests.
  • Identify where field-relative pose, heading, module state, and simulation behavior are handled.
  • Know what to inspect before changing drive constants or generated swerve configuration.

The Files We Will Use

Keep these files open while reading:

What The Drivetrain Does

The drivetrain is the system that moves the robot chassis. On this robot, the drivetrain is a swerve drive.

A swerve drive has four modules. Each module has:

  • a drive motor for wheel speed,
  • a steer motor for wheel angle,
  • an encoder for module angle.

The driver thinks in chassis motion:

drive forward/backward
drive left/right
rotate clockwise/counterclockwise

The CTRE swerve library turns those chassis requests into module states:

front-left wheel speed and angle
front-right wheel speed and angle
back-left wheel speed and angle
back-right wheel speed and angle

Our code is responsible for choosing the right request, shaping inputs, selecting assisted modes, handling autonomous ownership, and logging enough data to debug behavior.

Current Drivetrain Architecture

The drivetrain code is split into clear layers:

Robot
  owns robot lifecycle and mode transitions

RobotContainer
  constructs drivetrain objects and calls periodic methods

Drive
  selects the active drive mode

Manual
  turns driver sticks into field-centric swerve requests

ThetaLock
  combines driver translation with a controlled heading

AutoAlign
  drives toward a target field pose

Autos
  sends path-following speeds into the drivetrain request path

CommandSwerveDrivetrain
  stores and applies the active CTRE SwerveRequest

TunerConstants
  stores generated swerve hardware configuration

The important design choice is that RobotContainer does not own drive behavior. It wires the objects together. Drive behavior lives in the drive package.

High-Level Code Flow

Here is the normal teleop drive path:

flowchart TD
  A[Driver moves controller sticks] --> B[Controllers.TROY axes and triggers]
  B --> C[Robot.robotPeriodic]
  C --> D[RobotContainer.periodic]
  D --> E[Drive.periodic]
  E --> F{Drive.Mode}
  F -->|MANUAL| G[Manual.periodic]
  F -->|THETA_LOCK| H[ThetaLock.periodic]
  F -->|AUTO_ALIGN| I[AutoAlign.periodic]
  F -->|NONE| J[SwerveDriveBrake or Idle request]
  G --> K[drivetrain.setRequest]
  H --> K
  I --> K
  J --> K
  K --> L[CommandSwerveDrivetrain.periodic]
  L --> M[setControl request]
  M --> N[CTRE swerve calculates module targets]
  N --> O[TalonFX / CANcoder / Pigeon hardware]
  O --> P[logged pose, speeds, module states, currents]
Loading

The key idea is request ownership. A drive mode creates a SwerveRequest and writes it with:

drivetrain.setRequest(curRequest);

Then CommandSwerveDrivetrain.periodic() applies the stored request:

setControl(request);

That means you should ask two questions when debugging:

  1. Who wrote the current request?
  2. What request did CommandSwerveDrivetrain apply?

Detailed Code Flow

Step 1: Driver Input Starts In Controllers

Open Controllers.java.

The driver controller is:

public static final CommandXboxController TROY = new CommandXboxController(0);

Manual drivetrain code reads:

Controllers.TROY.getLeftY()
Controllers.TROY.getLeftX()
Controllers.TROY.getRightX()

Those are raw axis values from the Driver Station. They are not yet velocities. They are unitless stick inputs.

Drive mode can also depend on triggers:

public static final Trigger SHOOTING = SHOOT_REDUNDANCY.or(SHOOT);
public static final Trigger SHUTTLING = LEFT_SHUTTLE.or(RIGHT_SHUTTLE).and(SHOOTING.negate());

Those triggers affect the manual drive scaling.

Step 2: Robot Controls Mode Transitions

Open Robot.java.

The robot lifecycle sets high-level drivetrain mode:

public void disabledInit() {
  robotContainer.drivetrainController.setDriveMode(Drive.Mode.NONE);
}
public void autonomousInit() {
  robotContainer.drivetrainController.setDriveMode(Drive.Mode.NONE);
}
public void teleopInit() {
  robotContainer.drivetrainController.setDriveMode(Drive.Mode.MANUAL);
}

This matters because manual drive should not fight autonomous path following. During autonomous, the drive controller is set to NONE, and the autonomous command owns the drivetrain request.

Step 3: RobotContainer Runs The Periodic Order

Open RobotContainer.java.

The drivetrain objects are constructed here:

public final Drive drivetrainController;
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();

Then:

drivetrainController = new Drive(drivetrain);

The periodic order is:

public void periodic() {
  superstructure.periodic();
  drivetrainController.periodic();
  drivetrain.periodic();
}

Read that order as:

superstructure updates mechanism goals
  -> drive controller chooses the active drivetrain request
  -> CTRE drivetrain applies the request and records measured state

If a drive mode sets a request after drivetrain.periodic() has already run, that request would not be applied until the next cycle. Periodic order is part of behavior.

Step 4: Drive Chooses The Active Mode

Open Drive.java.

The active mode is:

public enum Mode {
  NONE,
  MANUAL,
  THETA_LOCK,
  AUTO_ALIGN;
}

Every cycle, Drive.periodic() clears child goals:

manual.setGoal(Manual.Goal.NONE);
thetaLock.setGoal(ThetaLock.Goal.NONE);
autoAlign.setGoal(AutoAlign.Goal.NONE);

Then it activates the child that matches the current mode:

switch (mode) {
  case MANUAL -> manual.setGoal(Manual.Goal.ACTIVE);
  case AUTO_ALIGN -> autoAlign.setGoal(AutoAlign.Goal.ALIGN);
  case THETA_LOCK -> thetaLock.setGoal(ThetaLock.Goal.ACTIVE);
  default -> {}
}

Then each child runs:

manual.periodic();
thetaLock.periodic();
autoAlign.periodic();

This pattern makes mode ownership explicit. Only the active child should write the drivetrain request.

Step 5: Manual Builds A Field-Centric Drive Request

Open Manual.java.

Manual drive turns raw stick inputs into a SwerveRequest.FieldCentric.

The shaping chain is:

JOYSTICK_VALUES
    .setValues(
        Controllers.TROY.getLeftY(),
        Controllers.TROY.getLeftX(),
        Controllers.TROY.getRightX())
    .exponentialCurve(TRANSLATION_EXP_CURVE, ROTATION_EXP_CURVE)
    .scale(-MAX_SPEED, -MAX_ANGULAR_RATE)
    .slewRateLimit(X_LIMITER, Y_LIMITER, ROT_LIMITER);

That does four things:

  1. Reads raw axes.
  2. Applies an exponential curve for finer low-speed control.
  3. Scales unitless input into meters per second and radians per second.
  4. Applies slew-rate limiting.

Manual drive chooses between normal, shooting, and shuttling requests:

if (Controllers.SHUTTLING.getAsBoolean()) {
  curRequest = shuttlingDrive.withVelocityX(...);
} else if (Controllers.SHOOTING.getAsBoolean()) {
  curRequest = shootingDrive.withVelocityX(...);
} else {
  curRequest = drive.withVelocityX(...);
}

Finally, it writes the selected request:

drivetrain.setRequest(curRequest);

This is driver-feel code. If the robot feels too fast, too slow, too sensitive near zero, or too jerky, inspect Manual before editing hardware constants.

Step 6: ThetaLock Replaces Free Rotation With Controlled Heading

Open ThetaLock.java.

Theta lock still uses manual translation:

.withVelocityX(manual.getValues().getX())
.withVelocityY(manual.getValues().getY())

But it uses FieldCentricFacingAngle to control heading:

.withTargetDirection(desiredRotation.get())

So theta lock is not a separate drivetrain. It is a different request mode:

driver controls X/Y translation
code controls robot heading

When debugging theta lock, inspect:

  • Drive.Mode,
  • ThetaLock.Goal,
  • target angle,
  • current angle,
  • rotation error,
  • heading PID gains,
  • manual X/Y values.

Step 7: AutoAlign Drives Toward A Field Pose

Open AutoAlign.java.

Auto-align reads the drivetrain pose:

Pose2d currentPose = drivetrain.getState().Pose;

It chooses a target pose:

Pose2d targetPose = ...

It computes translation error:

Translation2d translationError = targetTranslation.minus(currentTranslation);
double currentDistanceToTarget = translationError.getNorm();

Then it creates a field-centric facing-angle request:

return driveAtAngle
    .withVelocityX(translationOutput.getX())
    .withVelocityY(translationOutput.getY())
    .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance)
    .withTargetDirection(targetPose.getRotation());

Auto-align depends on pose correctness. If the robot pose is wrong, auto-align will drive to the wrong place even if the PID code is correct.

Step 8: CommandSwerveDrivetrain Applies The Stored Request

Open CommandSwerveDrivetrain.java.

The stored request starts idle:

private SwerveRequest request = new SwerveRequest.Idle();

Other code writes a request with:

public void setRequest(SwerveRequest request) {
  this.request = request;
}

Then periodic() applies it:

setControl(request);

This is the boundary between team-owned drive mode logic and CTRE's drivetrain implementation.

CommandSwerveDrivetrain also owns:

  • operator perspective handling,
  • sim pose injection,
  • drivetrain telemetry,
  • module target and measured state logging,
  • IMU logging,
  • current logging,
  • vision measurement forwarding,
  • bump detection.

Step 9: CTRE Applies Module Targets

After setControl(request), CTRE's swerve library handles the low-level swerve calculations.

The CTRE layer uses constants from TunerConstants.java, including:

  • drive motor IDs,
  • steer motor IDs,
  • CANcoder IDs,
  • encoder offsets,
  • module positions,
  • wheel radius,
  • gear ratios,
  • Pigeon ID,
  • drive and steer gains,
  • inversion,
  • current limits.

If one module behaves differently from the others, inspect TunerConstants and the logged target/measured module states before changing driver-control code.

Autonomous Code Flow

Autonomous uses the same stored-request mechanism, but the request comes from path following instead of manual drive.

flowchart TD
  A[autonomousInit] --> B[Drive mode set to NONE]
  A --> C[autos.getAutonomousCommand]
  C --> D[B-Line FollowPath command]
  D --> E[Path follower computes ChassisSpeeds]
  E --> F[ApplyRobotSpeeds request]
  F --> G[drivetrain.setRequest]
  G --> H[CommandSwerveDrivetrain.periodic]
  H --> I[setControl request]
  I --> J[CTRE swerve output]
Loading

Open Autos.java.

The path builder writes robot speeds into the drivetrain:

speeds -> drivetrain.setRequest(pathApplyRobotSpeeds.withSpeeds(speeds))

Open Robot.java.

Autonomous disables manual drive ownership:

robotContainer.drivetrainController.setDriveMode(Drive.Mode.NONE);

That prevents Manual.periodic() from writing over the autonomous path request.

If an auto does not drive, inspect:

  • selected auto routine,
  • whether the auto command is scheduled,
  • whether Drive.Mode is NONE,
  • whether the path follower calls drivetrain.setRequest(...),
  • current pose and start pose,
  • whether CommandSwerveDrivetrain.periodic() is applying the request.

Simulation Code Flow

Simulation still runs through CommandSwerveDrivetrain.

When the drivetrain is constructed in sim:

if (Utils.isSimulation()) {
  startSimThread();
}

startSimThread() creates MapleSimSwerveDrivetrain and runs it periodically:

simNotifier = new Notifier(mapleSimSwerveDrivetrain::update);
simNotifier.startPeriodic(SIM_LOOP_PERIOD);

Then CommandSwerveDrivetrain.periodic() pulls the simulated pose back into CTRE's drivetrain state:

Pose2d simPose = mapleSimSwerveDrivetrain.mapleSimDrive.getSimulatedDriveTrainPose();
super.resetPose(simPose);

Simulation is useful for validating request flow, autonomous scheduling, field pose behavior, and logs. It does not prove real CAN IDs, encoder offsets, module inversion, wheel slip, or driver feel.

Logging And Debug Signals

CommandSwerveDrivetrain.periodic() records the drivetrain signals that matter most:

Logger.recordOutput("Drive/Pose", getState().Pose);
Logger.recordOutput("Drive/TargetStates", getState().ModuleTargets);
Logger.recordOutput("Drive/MeasuredStates", getState().ModuleStates);
Logger.recordOutput("Drive/MeasuredSpeeds", getState().Speeds);

It also logs IMU and current values.

Manual drive logs raw joystick values:

Logger.recordOutput("Drive/Manual/Joystick/X", Controllers.TROY.getLeftY());
Logger.recordOutput("Drive/Manual/Joystick/Y", Controllers.TROY.getLeftX());
Logger.recordOutput("Drive/Manual/Joystick/Rot", Controllers.TROY.getRightX());

Theta lock logs heading state.

Auto-align logs target pose, current pose, translation output, angle to target, and alignment errors.

Use these logs before editing:

  • speed scales,
  • PID gains,
  • module offsets,
  • inversion,
  • wheel radius,
  • gear ratios.

What To Inspect For Drive Issues

Use this order:

  1. Driver Station mode and enable state.
  2. Drive.Mode: NONE, MANUAL, THETA_LOCK, or AUTO_ALIGN.
  3. Active child goal: Manual.Goal, ThetaLock.Goal, or AutoAlign.Goal.
  4. Who last called drivetrain.setRequest(...).
  5. Raw joystick values.
  6. Shaped manual values.
  7. Request type: field-centric, facing-angle, brake, idle, or apply-robot-speeds.
  8. Drivetrain pose.
  9. Target module states.
  10. Measured module states.
  11. IMU yaw, pitch, and roll.
  12. Drive current and battery voltage.
  13. TunerConstants only after the behavior points to hardware configuration.

Common Symptoms

Robot does not move in teleop:

  • Check that the robot is enabled.
  • Check that Drive.Mode is MANUAL.
  • Check joystick logs.
  • Check that Manual.Goal is ACTIVE.
  • Check whether Manual calls drivetrain.setRequest(...).

Robot moves too slowly:

  • Check whether shooting or shuttling mode is active.
  • Check SHOOTING_SPEED and SHUTTLING_SPEED.
  • Check slew limiters.
  • Check battery voltage and current limits.

Robot rotates when it should hold angle:

  • Check whether ThetaLock or AutoAlign is active.
  • Check target angle and current angle.
  • Check heading PID gains.
  • Check gyro yaw and operator perspective.

Auto-align drives to the wrong place:

  • Check current pose.
  • Check target pose.
  • Check alliance flipping.
  • Check coordinate frames.
  • Check whether vision or odometry is stale.

Autonomous path does not drive:

  • Check selected auto.
  • Check whether the autonomous command is scheduled.
  • Check that Drive.Mode is NONE.
  • Check whether the path follower writes drivetrain.setRequest(...).
  • Check pose reset and start pose.

One module behaves differently:

  • Compare target module state to measured module state.
  • Check that module's drive motor ID, steer motor ID, encoder ID, encoder offset, and inversion.
  • Check CAN wiring and device health.

Why This Architecture Helps

The current drivetrain architecture separates responsibilities:

  • Robot owns mode transitions.
  • RobotContainer wires objects and periodic order.
  • Drive owns active drive mode.
  • Manual owns driver input shaping.
  • ThetaLock owns heading lock.
  • AutoAlign owns target-pose alignment.
  • Autos owns path-following requests.
  • CommandSwerveDrivetrain owns CTRE request application and telemetry.
  • TunerConstants owns generated hardware constants.

That separation makes drivetrain code easier to extend. Assisted drive behavior can be added in the drive package without turning RobotContainer into a large drive-state file.

Exercise

Work through the code in this order:

  1. Open RobotContainer.java.
  2. Find where CommandSwerveDrivetrain and Drive are constructed.
  3. Find the periodic order.
  4. Open Drive.java and explain how Mode selects child goals.
  5. Open Manual.java and trace raw joystick input into a SwerveRequest.
  6. Find where shooting and shuttling change speed limits.
  7. Open ThetaLock.java and explain how it combines manual translation with controlled heading.
  8. Open AutoAlign.java and identify current pose, target pose, translation output, and target direction.
  9. Open CommandSwerveDrivetrain.java and find setRequest(...) and setControl(request).
  10. Open Autos.java and find where path speeds become a drivetrain request.
  11. Open Robot.java and identify how disabled, autonomous, and teleop choose drive modes.

When you are done, explain manual drive:

In teleop manual drive, controller input starts in ___.
Manual drive shapes it using ___.
The selected request is written through ___.
The CTRE drivetrain applies it in ___.
Logs for target and measured module states come from ___.

Then explain autonomous:

In autonomous, Drive.Mode is set to ___ so manual control does not fight path following.
The path command writes requests through ___.
The stored request is applied by ___.

Checkoff

Be ready to answer:

  • What does Drive.Mode control?
  • Why does RobotContainer not build the manual drive request anymore?
  • What does Manual own?
  • What does ThetaLock add on top of manual drive?
  • What does AutoAlign need from drivetrain pose?
  • What does CommandSwerveDrivetrain.setRequest(...) do?
  • Where is the stored request applied to CTRE?
  • How do autos write drivetrain requests?
  • Which logs would you inspect for a drive issue?
  • Which constants should not be edited casually?

Before Next Lesson

Review autonomous code with this request flow in mind. Autos are not separate from drivetrain behavior. They write drivetrain requests through the same CommandSwerveDrivetrain request path, but they must own the request while manual driving is inactive.

Clone this wiki locally