Video course · Lesson 2 75 min

Your First Autonomous

Write an Autonomous OpMode that drives exact distances with encoders, turns with the IMU and always finishes thanks to safety timeouts.

VIDEO COMING SOON

Our team is preparing this lesson. Written materials and practical exercises are available below.

In this lesson you will

Create an @Autonomous OpMode that reuses your TeleOp hardware setup
Calculate ticks per inch from the motor datasheet and wheel size
Write a reusable driveInches() helper using RUN_TO_POSITION
Turn to a heading using the IMU yaw angle and a simple proportional controller
Add ElapsedTime timeouts and test the routine for repeatability

Lesson

In the Autonomous period no one touches the gamepads, so the robot must know how far it has driven and which way it is facing on its own. In this lesson you will build an Autonomous OpMode that drives 24 inches, turns 90 degrees left and drives 12 more inches. You will reuse the same motor names and directions from your TeleOp.

Step 1: From ticks to inches

Each drive motor has an encoder that counts ticks as the shaft turns. To drive a distance, you need to know how many ticks equal one inch of travel. That depends on three things: the encoder ticks per motor revolution, any gearing between the motor and the wheel, and the wheel diameter.

Ticks per wheel revolution divided by wheel circumference gives ticks per inch.

JAVA

1// EXAMPLE VALUES ONLY - find your motor's encoder ticks per output
2// revolution on its datasheet (the manufacturer's product page).
3static final double TICKS_PER_MOTOR_REV = 537.7;  // example: goBILDA 312 RPM motor
4static final double DRIVE_GEAR_REDUCTION = 1.0;   // 1.0 = wheel mounted directly on motor
5static final double WHEEL_DIAMETER_INCHES = 3.78; // example: 96 mm wheel
6
7static final double TICKS_PER_INCH =
8        (TICKS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * Math.PI);
9// With these example numbers: 537.7 / (3.78 * 3.1416) = about 45.3 ticks per inch

Example constants. Replace them with the values for your own motors and wheels.

Step 2: Set up the Autonomous OpMode

The structure looks like your TeleOp, but the annotation is @Autonomous and, instead of a gamepad loop, runOpMode() calls helper methods in order. The motors and IMU are stored as class fields so every helper can use them. Mount-specific settings, like the hub orientation, must describe how your Control Hub is actually mounted.

JAVA

1package org.firstinspires.ftc.teamcode;
2
3import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
4import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
5import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
6import com.qualcomm.robotcore.hardware.DcMotor;
7import com.qualcomm.robotcore.hardware.IMU;
8import com.qualcomm.robotcore.util.ElapsedTime;
9import com.qualcomm.robotcore.util.Range;
10import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
11
12@Autonomous(name = "First Auto", group = "Training")
13public class FirstAuto extends LinearOpMode {
14    private DcMotor frontLeft, backLeft, frontRight, backRight;
15    private DcMotor[] motors;
16    private IMU imu;
17
18    @Override
19    public void runOpMode() {
20        frontLeft = hardwareMap.get(DcMotor.class, "front_left");
21        backLeft = hardwareMap.get(DcMotor.class, "back_left");
22        frontRight = hardwareMap.get(DcMotor.class, "front_right");
23        backRight = hardwareMap.get(DcMotor.class, "back_right");
24        frontLeft.setDirection(DcMotor.Direction.REVERSE); // same as TeleOp
25        backLeft.setDirection(DcMotor.Direction.REVERSE);
26        motors = new DcMotor[] {frontLeft, backLeft, frontRight, backRight};
27        for (DcMotor m : motors) m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
28
29        imu = hardwareMap.get(IMU.class, "imu");
30        imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(
31                RevHubOrientationOnRobot.LogoFacingDirection.UP,       // change to match
32                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD))); // your mounting
33
34        telemetry.addData("Status", "Ready - press START");
35        telemetry.update();
36        waitForStart();
37        imu.resetYaw(); // the starting direction becomes 0 degrees
38
39        driveInches(24, 0.5, 4.0);   // distance, power, timeout in seconds
40        turnToHeading(90, 0.5, 3.0); // positive angle = turn left
41        driveInches(12, 0.5, 3.0);
42    }
43}

FirstAuto.java: setup and the routine. Place the constants from Step 1 inside this class.

A reusable drive helper. The RUN_TO_POSITION mode lets the motor controller do the hard work: you give each motor a target in ticks and a maximum power, and the controller drives the motor to that position. The order matters: reset the encoders, set the target, and only then switch to RUN_TO_POSITION, otherwise the SDK reports an error. Add this method inside the FirstAuto class.

JAVA

1private void driveInches(double inches, double power, double timeoutS) {
2    int target = (int) Math.round(inches * TICKS_PER_INCH);
3    for (DcMotor m : motors) {
4        m.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
5        m.setTargetPosition(target);              // set the target first
6        m.setMode(DcMotor.RunMode.RUN_TO_POSITION);
7    }
8    ElapsedTime timer = new ElapsedTime();
9    for (DcMotor m : motors) m.setPower(Math.abs(power)); // direction comes from the target
10
11    while (opModeIsActive() && timer.seconds() < timeoutS
12            && frontLeft.isBusy() && backLeft.isBusy()
13            && frontRight.isBusy() && backRight.isBusy()) {
14        telemetry.addData("Target ticks", target);
15        telemetry.addData("FL / FR", "%d / %d",
16                frontLeft.getCurrentPosition(), frontRight.getCurrentPosition());
17        telemetry.addData("Time", "%.1f of %.1f s", timer.seconds(), timeoutS);
18        telemetry.update();
19    }
20    for (DcMotor m : motors) {
21        m.setPower(0);
22        m.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
23    }
24}

Drives all four wheels the same number of ticks, with a timeout.

Step 3: Turn with the IMU

The Control Hub has a built-in IMU that measures rotation. Its yaw angle is your robot's heading: 0 at the start (after resetYaw()), increasing when the robot turns left (counter-clockwise seen from above). To turn, you repeatedly compare the target heading with the current heading. The difference is the error, and turning power proportional to the error makes the robot slow down as it gets close. This is a simple feedback loop called proportional control.

Target heading minus measured heading gives the error, which sets the turn power.

JAVA

1static final double TURN_GAIN = 0.02;      // power per degree of error - tune this
2static final double MIN_TURN_POWER = 0.1;  // enough to actually move the robot
3static final double HEADING_TOLERANCE = 2.0; // degrees
4
5private double getHeading() {
6    return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
7}
8
9private void turnToHeading(double targetDeg, double maxPower, double timeoutS) {
10    ElapsedTime timer = new ElapsedTime();
11    while (opModeIsActive() && timer.seconds() < timeoutS) {
12        double error = targetDeg - getHeading();
13        while (error > 180) error -= 360;   // always take the shorter way round
14        while (error <= -180) error += 360;
15        if (Math.abs(error) < HEADING_TOLERANCE) break;
16
17        double turn = Range.clip(error * TURN_GAIN, -maxPower, maxPower);
18        if (Math.abs(turn) < MIN_TURN_POWER) turn = Math.signum(turn) * MIN_TURN_POWER;
19        // Positive turn = left: left wheels backward, right wheels forward
20        frontLeft.setPower(-turn);
21        backLeft.setPower(-turn);
22        frontRight.setPower(turn);
23        backRight.setPower(turn);
24
25        telemetry.addData("Heading", "%.1f (target %.1f)", getHeading(), targetDeg);
26        telemetry.addData("Error", "%.1f", error);
27        telemetry.update();
28    }
29    for (DcMotor m : motors) m.setPower(0);
30}

Proportional turn to an absolute heading. Add these inside the FirstAuto class.

Timeouts: every loop also checks an ElapsedTime timer with timer.seconds() < timeoutS. If a wheel slips, the robot hits a wall or the target can never be reached, the loop still ends and the routine moves on instead of freezing for the rest of the period.
Stop checks in every loop: calling opModeIsActive() lets the OpMode stop immediately when STOP is pressed or the period ends.
Hub orientation: if yaw does not increase when you rotate the robot left by hand, the LogoFacingDirection and UsbFacingDirection values do not match your mounting. Fix them before tuning anything else.
Choosing timeouts: set each timeout a little longer than the move normally takes, and keep the whole routine well inside the 30-second Autonomous period (check the current Competition Manual for exact match timing).

Step 4: Test for repeatability

1
Check the sensors
Run the OpMode with the robot on a box. Push the wheels and rotate the robot by hand in INIT to confirm encoders count up when driving forward and yaw increases when turning left.
2
Calibrate ticks per inch
Call driveInches(48, 0.4, 6.0) and measure the real distance. New value = old TICKS_PER_INCH x 48 / measured inches. Repeat until it is within about half an inch.
3
Tune the turn
If the robot overshoots and wobbles, lower TURN_GAIN. If it stops short or times out, raise MIN_TURN_POWER slightly.
4
Run it five or more times
Tape the start position, run the full routine, and mark where the robot stops each time. Record the distance from the target, the heading error and the battery voltage in a table. If results vary, look for low battery, wheel slip or a different start position, and change only one value between runs.

Key terms

Encoder tick: One count from a motor's encoder; a fixed number of ticks makes one full revolution of the output shaft.
RUN_TO_POSITION: A motor mode in which the motor controller drives the motor to a target tick count and holds it there.
IMU yaw: The robot's heading angle measured by the IMU, which increases as the robot turns left.
Proportional control: A feedback method where the correction power is the error multiplied by a gain, so the robot slows as it nears the target.
Timeout: A time limit, measured with ElapsedTime, that ends a loop even if the target was never reached.

Practical exercise

Build, calibrate and extend your first Autonomous

1Look up your drive motor's encoder ticks per output revolution and your wheel diameter, and calculate ticks per inch on paper.
2Build FirstAuto with driveInches() and turnToHeading() and check the sensors with the robot on a box.
3Calibrate TICKS_PER_INCH with a 48-inch test drive and a tape measure.
4Extend the routine to drive a full square: four drives of 24 inches and turns to 90, 180, -90 and 0 degrees.
5Run the square five times from a taped start and record how far from the start position the robot finishes each time.

Deliverable: A working FirstAuto OpMode that drives a 24-inch square, plus a table of five runs showing the final position and heading error.

Further resources

Mark lesson complete

In this lesson you will

Lesson

Step 1: From ticks to inches

Step 2: Set up the Autonomous OpMode

Step 3: Turn with the IMU

Step 4: Test for repeatability

Key terms

Practical exercise

Build, calibrate and extend your first Autonomous

Knowledge check

Further resources