Unit 7 · Lesson 9

Field-Oriented Controls

One method call separates robot-oriented from field-oriented. ChassisSpeeds.fromFieldRelativeSpeeds() is six words and nine parameters, but it unlocks the single most powerful competitive advantage in FRC driving. This lesson explains the math behind it, upgrades the DriveCommand from Lesson 8 with a one-line change, and covers everything that can go wrong — and how to handle it.

By the end of this lesson, you will:

Explain why field-oriented control reduces driver cognitive load and produces faster cycle times
Describe the 2D rotation matrix that fromFieldRelativeSpeeds() applies internally
Upgrade the Lesson 8 DriveCommand to field-oriented with the single required change
Implement a gyro-failsafe toggle that switches between field-oriented and robot-oriented at runtime
Handle alliance color flipping so field-oriented "forward" always means toward the opponent wall
Explain the gyro zero button's role as the most critical driver interface element on a swerve robot

The Competitive Case for Field-Oriented

In a real match, the robot gets hit. It spins during a defensive engagement, ends up sideways after a collision, gets pushed off-target during an intake cycle. With robot-oriented control, every time this happens, the driver's mental model breaks: "forward" now points somewhere wrong. Under competition adrenaline and time pressure, the driver hesitates, miscorrects, wastes a half-second. Against an alliance running field-oriented, that hesitation is the difference between a completed cycle and a missed shot.

Field-oriented control eliminates this entirely. The joystick always means the same thing: toward the far wall, away from the far wall, left across the field, right across the field. Robot heading becomes irrelevant to the driver's translation decisions. They think in field coordinates — the natural coordinate system of a player looking down at the field — and the code handles the rotation.

🔍 LRI Perspective: "I can identify the control mode in the first five seconds of a match"

Watch a driver at competition for five seconds. If the robot hesitates after a collision or drives at an angle when they push straight, that's robot-oriented. If the robot snaps back to the driver's intended direction immediately after a hit, that's field-oriented. The difference isn't visible in the code — it's visible in the driver's confidence. 2910 drivers don't think about the robot's heading during TeleOp. They think about the field. That's the mental space field-oriented control buys them, and it compounds into real cycle time advantages over a match.

The Rotation Matrix: One Paragraph of Geometry

Field-oriented control is 2D vector rotation. The driver's joystick produces a velocity vector in field coordinates — (vx_field, vy_field). The robot's current heading θ is known from the gyro. To produce the same physical field-relative motion, the robot must be commanded with that vector rotated by −θ into the robot's reference frame. This is a standard 2D rotation:

2D rotation transformation — field → robot frame

vx_robot = vx_field·cos(θ) + vy_field·sin(θ) Forward component of robot velocity — combination of field forward/strafe weighted by heading angle

vy_robot = −vx_field·sin(θ) + vy_field·cos(θ) Lateral component of robot velocity — the perpendicular combination

omega unchanged Rotation is frame-invariant — spinning at 1 rad/s looks the same in both frames

You don't need to implement this math yourself. ChassisSpeeds.fromFieldRelativeSpeeds() applies it internally. But understanding it is what lets you debug when field-oriented goes wrong — if the robot curves instead of going straight, θ is wrong. If axes are mirrored, θ has the wrong sign. The math is the map to the bug.

Field-Oriented vs. Robot-Oriented: The Difference Made Visible

Select a robot heading below. The joystick is always pushed forward (+vx in field coordinates). Watch how the module states and the resulting robot motion differ between the two control modes.

Field-oriented comparison — joystick always pushed forward

Robot heading:

Joystick input:vx_joy = 1.0

Gyro heading θ:0°

🤖 RO vx/vy:+1.0 / 0.0

RO motion:Robot drives forward

🏟️ FO vx/vy:+1.0 / 0.0

FO motion:Always toward red wall

← select a robot heading to see how field-oriented keeps "forward" pointing toward the opponent wall

The One-Line Change: Robot-Oriented → Field-Oriented

This is the complete transformation of the Lesson 8 DriveCommand into a field-oriented drive command. Every line is identical except the ChassisSpeeds construction.

DriveCommand.java — upgrading to field-oriented (only the execute() method shown)

// All axis reading and deadband code is identical to Lesson 8...

// ── Old: robot-oriented ──────────────────────────────
−m_drive.drive(new ChassisSpeeds(vx, vy, omega));

// ── New: field-oriented ───────────────────────────────
+m_drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(vx, vy, omega, m_drive.getHeading()));

That's the complete change. One constructor call replaced with one static factory call that takes an additional argument: the robot's current heading from the gyro. The rest of the pipeline — discretize(), toSwerveModuleStates(), desaturateWheelSpeeds(), four setDesiredState() calls — is identical. The architecture held exactly as designed.

The Complete FieldOrientedDriveCommand

FieldOrientedDriveCommand.java — production version with failsafe toggle

public class FieldOrientedDriveCommand extends CommandBase {

  private final SwerveDrivetrain m_drive;
  private final CommandXboxController m_controller;

  // Failsafe: toggle to robot-oriented if gyro unreliable
  private boolean m_fieldOriented = true;

  public FieldOrientedDriveCommand(SwerveDrivetrain drive,
                                       CommandXboxController controller) {
    m_drive = drive;
    m_controller = controller;
    addRequirements(drive);
  }

  @Override
  public void execute() {
    double vx = -MathUtil.applyDeadband(m_controller.getLeftY(), 0.05)
      * Constants.Swerve.kMaxSpeedMetersPerSecond;
    double vy = -MathUtil.applyDeadband(m_controller.getLeftX(), 0.05)
      * Constants.Swerve.kMaxSpeedMetersPerSecond;
    double omega = -MathUtil.applyDeadband(m_controller.getRightX(), 0.05)
      * Constants.Swerve.kMaxAngularRadPerSecond;

    ChassisSpeeds speeds;
    if (m_fieldOriented) {
      speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
        vx, vy, omega, m_drive.getHeading());
    } else {
      // Robot-oriented fallback — no gyro needed
      speeds = new ChassisSpeeds(vx, vy, omega);
    }
    m_drive.drive(speeds);

    // Publish active mode for driver awareness
    SmartDashboard.putBoolean("Drive/FieldOriented", m_fieldOriented);
  }

  /** Called by a button binding to toggle the control mode */
  public void toggleFieldOriented() {
    m_fieldOriented = !m_fieldOriented;
  }

  @Override
  public void end(boolean interrupted) { m_drive.stop(); }
  @Override
  public boolean isFinished() { return false; }
}

← click a highlighted token

Wiring in RobotContainer

RobotContainer.java — field-oriented wiring with failsafe and gyro zero

private final FieldOrientedDriveCommand m_driveCmd =
  new FieldOrientedDriveCommand(m_drive, m_driver);

private void configureDefaultCommands() {
  m_drive.setDefaultCommand(m_driveCmd);
}

private void configureButtonBindings() {

  // Zero gyro heading — most important button on the robot
  m_driver.start().onTrue(
    Commands.runOnce(m_drive::zeroHeading, m_drive)
    .withName("ZeroHeading"));

  // Gyro failsafe: Back button toggles field/robot oriented
  m_driver.back().onTrue(
    Commands.runOnce(m_driveCmd::toggleFieldOriented)
    .withName("ToggleDriveMode"));

  // Lock wheels: X button hold (from Lesson 8)
  m_driver.x().whileTrue(
    Commands.run(m_drive::lockWheels, m_drive)
    .withName("LockWheels"));
}

← click a highlighted token

Alliance Color Flipping

WPILib's field coordinate system places the origin at the blue alliance wall. "Forward" in field coordinates (+X) always means toward the red alliance wall. For blue alliance drivers, this is natural — the driver is standing at the blue wall, and joystick-forward means toward the opponent.

For red alliance drivers, the driver is standing at the red wall. Joystick-forward should still mean toward their opponent — which is now the blue wall, the −X direction. Without a flip, red alliance drivers experience the field coordinates backward: pushing forward drives the robot away from the opponents instead of toward them.

The fix is a sign flip on the field-relative vx and vy components when on the red alliance:

FieldOrientedDriveCommand.java — alliance color flip

private boolean isRedAlliance() {
  return DriverStation.getAlliance()
    .map(a -> a == Alliance.Red)
    .orElse(false);
}

// In execute(), apply flip before fromFieldRelativeSpeeds:
double allianceFlip = isRedAlliance() ? -1.0 : 1.0;
double vxField = vx * allianceFlip;
double vyField = vy * allianceFlip;

ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  vxField, vyField, omega, m_drive.getHeading());
m_drive.drive(speeds);

// Heading zero must also account for alliance at match start.
// Red alliance: robot faces the blue wall → zeroHeading sets
// this as 0°. FO control then treats blue wall as "forward".
// The flip corrects this so joystick-fwd still means opponent.

← click a highlighted token

💡 PathPlanner handles the alliance flip automatically

PathPlanner's AutoBuilder.configure() accepts an alliance color supplier that it uses to mirror paths for the red alliance. For path-following autonomous, you don't need to apply the flip manually — the library does it. The flip described above is only needed for the driver-controlled TeleOp period. Double-check you're not applying the flip twice: once in the drive command and once in PathPlanner.

The Gyro Zero Button: Most Important Driver Control

The gyro zero button is not optional. It is not a nice-to-have. It is the recovery mechanism for every field-oriented failure mode.

Field-oriented control depends entirely on the gyro knowing which direction the field is. At match start, the robot is placed at a known orientation and the gyro is zeroed to that orientation. This works perfectly — until a defense collision spins the robot and the driver's spatial reference drifts slightly, or a gyro initialization issue means the starting heading is subtly wrong, or the driver simply loses track after a fast rotation.

The gyro zero button tells the robot: "My front is now facing the correct forward direction. Reset your reference." After pressing it, field-oriented control is re-anchored. Every match, the 2910 driver protocol includes: at the start of TeleOp, before touching the joystick, press Start to zero the gyro with the robot facing its current orientation. This takes 0.2 seconds and prevents the most common source of field-oriented failure.

⚠️ Two common gyro zero mistakes

Mistake 1: Zeroing the gyro during robot motion. zeroHeading() calls m_pigeon.setYaw(0.0). If the robot is spinning when this fires, the zeroed heading is whatever random angle the robot happened to be at that moment. The gyro zero should always fire when the robot is stationary — use an .onTrue() binding so it fires once on button press, not .whileTrue() which would repeatedly zero while the button is held.

Mistake 2: Using zeroHeading() for odometry reset. Zeroing the gyro changes the heading reference but does NOT reset the Pose2d — after calling zeroHeading, the odometry's heading becomes inconsistent with its position. For odometry-accurate reset, call drivetrain.resetPose(new Pose2d(x, y, new Rotation2d())) which resets both heading and position together.

🔌 System Check — Field-Oriented Verification Protocol

After upgrading from robot-oriented (Lesson 8):

Basic forward test: Hold the robot at 0° heading. Push joystick forward — robot drives toward the red wall. Rotate robot 90° by hand (don't zero gyro). Push joystick forward again — robot should still drive toward the red wall, not toward the right wall. If it drives in the direction the robot is facing, field-oriented is not active.
Heading zero test: Rotate robot 90°. Press Start. Drive forward — robot should still drive toward the red wall (not where it's now facing). The heading zero only redefines which direction is "zero degrees" — it doesn't change the physical robot position.
Alliance flip test: Set DriverStation.getAlliance() to return Red (or use the DS alliance selector). Push joystick forward — robot should drive toward the blue wall. If it drives toward the red wall, the flip logic is not active for red alliance.
Failsafe toggle test: Press Back button to toggle to robot-oriented. Push joystick forward — robot drives in the direction its front is pointing. Press Back again — robot switches back to field-oriented. Confirm SmartDashboard's Drive/FieldOriented boolean matches the active mode.

Knowledge Check

1. The robot is facing 90° (pointing toward the right wall). The driver pushes the left stick forward (vx_joy = 1.0). In field-oriented mode, what ChassisSpeeds values are passed to drive()?

A vx=1.0, vy=0 — robot-oriented forward regardless of heading
B vx≈0, vy≈−1.0 — the rotation matrix converts field-forward into the robot's right direction. Since the robot is facing 90° right, to go field-forward it must actually command leftward strafe in its own frame.
C vx=−1.0, vy=0 — field-oriented reverses the input when rotated 90°
D vx=0.707, vy=0.707 — the input is split equally between axes

2. The gyro is working but the heading was never zeroed at match start — it powers on reporting 47°. The driver pushes forward. What does the robot do?

A The robot drives forward in the correct field direction — the FMS provides the starting orientation automatically.
B The robot drives in the field direction that corresponds to 47° from forward — slightly off-axis. "Forward" on the joystick maps to 47° rotated from the field's forward direction. The driver will need to compensate by angling the joystick slightly, which is exactly the cognitive load field-oriented should eliminate.
C The robot refuses to move because the gyro heading is out of the expected range.
D No effect — the gyro self-calibrates to 0° when the robot enables.

3. Mid-match, the Pigeon 2 encounters a hardware fault and begins reporting random yaw values that change rapidly. Without the gyro failsafe toggle, what is the robot's behavior in field-oriented mode?

A The robot stops moving — fromFieldRelativeSpeeds() returns zero speeds when the heading is invalid.
B The robot becomes uncontrollable — the rotation matrix is computed using the random heading value, so the resulting ChassisSpeeds point in random directions every 20 ms. The robot will spin and drive erratically regardless of joystick input. The failsafe toggle to robot-oriented mode immediately restores control since it bypasses the gyro entirely.
C The robot falls back to robot-oriented automatically — WPILib detects gyro faults and switches modes.
D The robot drives in a consistent but wrong direction — it uses the last valid gyro reading as a fallback.

💪 Practice Prompt

Field-Oriented Drive — Full Competition Build

Upgrade your DriveCommand by replacing the new ChassisSpeeds(vx, vy, omega) call with ChassisSpeeds.fromFieldRelativeSpeeds(vx, vy, omega, m_drive.getHeading()). Deploy and run the field-oriented verification protocol from the System Check above. Confirm all three directions (forward, strafe, rotation) behave correctly regardless of the robot's heading.
Add the m_fieldOriented toggle field and the toggleFieldOriented() method. In RobotContainer, bind the Back button to a Commands.runOnce(m_driveCmd::toggleFieldOriented). Test: with the robot rotated 45°, push forward in field-oriented mode → drives toward opponent wall. Toggle to robot-oriented → drives in the direction the robot is facing. Toggle back → field-oriented resumes.
Add the alliance color flip. Test both alliance colors by forcing DriverStation.getAlliance() to return Red (you can do this with the DS Alliance selector during testing). Confirm pushing forward on red alliance drives toward the blue wall, not the red wall.
Bind the gyro zero to Start (if not already done). Practice the full competition startup sequence: robot placed in starting position facing forward → enable → press Start → confirm Drive/HeadingDeg reads 0 → drive forward → confirm the robot goes the right direction. Do this five times until it's muscle memory for your driver.
Stretch goal: Add a RateLimiter to the omega input to smooth the rotation response: declare private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(Constants.Swerve.kMaxAngularRadPerSecond) and replace omega with m_rotLimiter.calculate(omega) in the execute method. Compare driving with and without the rate limiter — the limiter prevents sudden heading jerks and makes field-oriented feel more predictable at competition. This is a production quality-of-life feature on 2910 robots.