Lesson 12.3: Normalizing Angular Values for Field-Centric Driving

The Problem with Robot-Centric Control

In a standard robot-centric drive scheme, pushing the left joystick forward always drives the robot toward whatever direction the front of the robot is facing at that moment. If the robot has spun 90 degrees during a defensive maneuver, pushing "forward" on the joystick now drives the robot sideways from the driver's point of view. The driver must mentally account for the robot's rotation and compensate with stick inputs to achieve the intended field direction. Under match pressure, this mental overhead causes errors.

Field-centric control removes this burden entirely. Pushing the joystick forward always moves the robot away from the driver's station, regardless of which direction the robot's front is pointing. The robot's rotation is compensated for mathematically in the code before the motor commands are calculated. The driver's frame of reference is always the field, not the robot.

The Vector Rotation Calculation

Field-centric control works by rotating the driver's joystick input vector by the robot's current heading angle. If the robot is facing 90 degrees to the right, a "forward" joystick input needs to be rotated 90 degrees to produce a motion command that is actually field-forward.

The calculation uses two standard trigonometric functions from java.lang.Math:

Math.atan2(y, x) converts a Cartesian (x, y) coordinate into a polar angle (theta).
Math.hypot(x, y) computes the magnitude (length) of the vector.

The robot's current heading in radians is subtracted from the joystick's polar angle. This difference, correctedTheta, represents the direction the robot should actually move to achieve the driver's intended field direction. The corrected angle is then converted back to Cartesian coordinates to produce the final motor commands.

The Wraparound Problem and AngleUnit.normalizeRadians()

Subtracting the robot's heading from the joystick angle can produce a result outside the -π to π range. For example, if the joystick points at 2.8 radians and the robot is heading at -2.8 radians, the raw difference is 5.6 radians, which is well outside the range and would produce nonsensical motor outputs. The SDK provides AngleUnit.normalizeRadians(angle) to solve this. It takes any radian value and wraps it into the -π to π range by adding or subtracting 2π as needed. Always apply this to the subtraction result before converting back to Cartesian coordinates.

Annotated Code

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

@TeleOp(name="Field_Centric_Demo")
public class FieldCentricDemo extends OpMode {

    private DcMotor leftFront, rightFront, leftBack, rightBack;
    private IMU     imu;

    @Override
    public void init() {
        leftFront  = hardwareMap.get(DcMotor.class, "left_front");
        rightFront = hardwareMap.get(DcMotor.class, "right_front");
        leftBack   = hardwareMap.get(DcMotor.class, "left_back");
        rightBack  = hardwareMap.get(DcMotor.class, "right_back");

        rightFront.setDirection(DcMotor.Direction.REVERSE);
        rightBack.setDirection(DcMotor.Direction.REVERSE);

        imu = hardwareMap.get(IMU.class, "imu");
        imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(
            RevHubOrientationOnRobot.LogoFacingDirection.UP,
            RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
        )));
    }

    @Override
    public void loop() {
        double forward = -gamepad1.left_stick_y;
        double strafe  =  gamepad1.left_stick_x;
        double rotate  =  gamepad1.right_stick_x;

        // Get the robot's current heading in radians for the rotation calculation
        double robotHeading = imu.getRobotYawPitchRollAngles()
                                 .getYaw(AngleUnit.RADIANS);

        // Convert the joystick Cartesian coordinates to a polar angle and magnitude
        double joystickAngle = Math.atan2(forward, strafe);
        double magnitude     = Math.hypot(strafe, forward);

        // Subtract the robot's heading and normalize to keep the result in -PI to PI
        double correctedAngle = AngleUnit.normalizeRadians(joystickAngle - robotHeading);

        // Convert the corrected polar angle back to Cartesian for motor math
        double correctedStrafe  = Math.cos(correctedAngle) * magnitude;
        double correctedForward = Math.sin(correctedAngle) * magnitude;

        // Standard mecanum wheel mixing formula
        double lf = correctedForward + correctedStrafe + rotate;
        double rf = correctedForward - correctedStrafe - rotate;
        double lb = correctedForward - correctedStrafe + rotate;
        double rb = correctedForward + correctedStrafe - rotate;

        // Normalize if any value exceeds 1.0
        double max = Math.max(1.0, Math.max(Math.abs(lf),
                     Math.max(Math.abs(rf), Math.max(Math.abs(lb), Math.abs(rb)))));

        leftFront.setPower(lf  / max);
        rightFront.setPower(rf / max);
        leftBack.setPower(lb   / max);
        rightBack.setPower(rb  / max);

        telemetry.addData("Heading (deg)", "%.1f",
            imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
        telemetry.update();
    }
}

Fill-in-the-Blank Practice

The SDK utility method that wraps a radian value into the -π to π range is AngleUnit.__________().
To compute the polar magnitude of a joystick input vector, use the Java math method Math.__________(x, y).
In field-centric control, the robot's current heading is __________ from the joystick's polar angle before the corrected motor commands are calculated.

Show answers

normalizeRadians
hypot
subtracted

Template Challenge

Robot Scenario: A driver wants to test their field-centric math without a full mecanum drivetrain. Write a method called getFieldRelativeStrafe(double rawStrafe, double rawForward, double headingRadians) that returns only the corrected strafe component as a double. This isolates the vector rotation calculation so it can be unit-tested independently.

package org.firstinspires.ftc.teamcode;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

public class FieldCentricMath {

    // INSERT CODE HERE: Write the getFieldRelativeStrafe method.
    // It should compute the corrected polar angle from rawStrafe and rawForward,
    // subtract headingRadians, normalize the result,
    // and return Math.cos(correctedAngle) * magnitude.

}

Show answer

public static double getFieldRelativeStrafe(
        double rawStrafe, double rawForward, double headingRadians) {

    double joystickAngle  = Math.atan2(rawForward, rawStrafe);
    double magnitude      = Math.hypot(rawStrafe, rawForward);
    double correctedAngle = AngleUnit.normalizeRadians(joystickAngle - headingRadians);

    return Math.cos(correctedAngle) * magnitude;
}

Ready to move on?

The Problem with Robot-Centric Control​

The Vector Rotation Calculation​

The Wraparound Problem and AngleUnit.normalizeRadians()​

Annotated Code​

Fill-in-the-Blank Practice​

Template Challenge​