Field Centric Drive Code

What is Field Centric Drive?

If you imagine driving a robot normally, as long as the robot is facing forward relative to you, it drives normally. But if you turn the robot, instead of going forward relative to you, it goes forward relative to the robot. This can be bad since during comp it's difficult to keep track of the robot heading and everything else going on around you.

FCD (Field Centric Drive) makes it so the driver has one less thing to worry about during comp and makes it easier to drive. The controls always correspond to the field orientation, not the robot orientation.

The Code

To use the FCD code, create a new .java file named FieldCentricDriveNavX and paste the following:

package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
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.DcMotorEx;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;

import static com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.REVERSE;


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

    // NavX variable — receives data about robot direction
    NavxMicroNavigationSensor navx;

    // Motors
    DcMotorEx frontLeft;
    DcMotorEx backLeft;
    DcMotorEx frontRight;
    DcMotorEx backRight;

    // Joystick variables
    double lJoyY;
    double lJoyX;
    double rJoyX;

    // Joystick direction using atan2. Not strictly needed
    // but nice for debugging
    public static double joyDir(double joyX, double joyY) {
        return Math.atan2(joyY, joyX);
    }

    // Runs once when init is pressed
    public void init() {
        // Setting config names
        navx = hardwareMap.get(
            NavxMicroNavigationSensor.class, "imu");

        frontLeft = hardwareMap.get(
            DcMotorEx.class, "LeftFront");
        backLeft = hardwareMap.get(
            DcMotorEx.class, "LeftBack");
        backRight = hardwareMap.get(
            DcMotorEx.class, "RightBack");
        frontRight = hardwareMap.get(
            DcMotorEx.class, "RightFront");

        // Reverse motors so robot actually moves forward
        frontRight.setDirection(REVERSE);
        backRight.setDirection(REVERSE);

        // Brake when no power applied (prevents drifting)
        frontRight.setZeroPowerBehavior(
            DcMotor.ZeroPowerBehavior.BRAKE);
        frontLeft.setZeroPowerBehavior(
            DcMotor.ZeroPowerBehavior.BRAKE);
        backRight.setZeroPowerBehavior(
            DcMotor.ZeroPowerBehavior.BRAKE);
        backLeft.setZeroPowerBehavior(
            DcMotor.ZeroPowerBehavior.BRAKE);
    }


    public void loop() {
        // Get yaw (rotation) in radians
        double yaw = navx.getAngularOrientation(
            AxesReference.INTRINSIC,
            AxesOrder.XYZ,
            AngleUnit.RADIANS
        ).thirdAngle;

        // Read joystick values
        lJoyY = gamepad1.left_stick_y;
        lJoyX = gamepad1.left_stick_x;
        rJoyX = gamepad1.right_stick_x;

        // Counteract robot rotation with math
        double robotRotY =
            lJoyX * Math.sin(-yaw)
            + lJoyY * Math.cos(-yaw);
        double robotRotX =
            -lJoyY * Math.sin(-yaw)
            + lJoyX * Math.cos(-yaw);

        // Keep power ratios even if values exceed -1 to 1
        double motorPower = Math.max(
            Math.abs(robotRotY)
            + Math.abs(robotRotX)
            + Math.abs(rJoyX), 1);

        // Set motor powers
        frontLeft.setPower(
            (robotRotY + robotRotX + rJoyX) / motorPower);
        backLeft.setPower(
            (robotRotY - robotRotX + rJoyX) / motorPower);
        frontRight.setPower(
            (robotRotY - robotRotX - rJoyX) / motorPower);
        backRight.setPower(
            (robotRotY + robotRotX - rJoyX) / motorPower);

        // Telemetry for debugging
        telemetry.addData("Joy Dir: ",
            Math.toDegrees(joyDir(lJoyY, lJoyX)));
        telemetry.addData("Robot Dir: ",
            Math.toDegrees(yaw));
        telemetry.addData("Joystick Y: ", lJoyY);
        telemetry.addData("Joystick X: ", lJoyX);
        telemetry.addData("---", "---");
        telemetry.addData("Front Left: ",
            (robotRotY + robotRotX + rJoyX) / motorPower);
        telemetry.addData("Front Right: ",
            (robotRotY - robotRotX - rJoyX) / motorPower);
        telemetry.addData("Back Left: ",
            (robotRotY - robotRotX + rJoyX) / motorPower);
        telemetry.addData("Back Right: ",
            (robotRotY + robotRotX - rJoyX) / motorPower);

        telemetry.update();
    }
}