Knowing Which Way You Face: the IMU

Encoders tell you how much the wheels turned, but not always how much the robot turned. When a robot spins in place, its wheels skid sideways a little, so encoder-based turns drift. The IMU (Inertial Measurement Unit) measures the rotation of the robot body itself, which makes it the right tool for turning and for checking that you are driving straight. Because it is already inside the Control Hub, you do not need to buy or wire anything extra.

Yaw, pitch and roll

• Yaw: rotation around the vertical axis, like a spinning top. This is the robot's heading.
• Pitch: tilting the front up or down, like going up a ramp.
• Roll: tilting left or right, like starting to tip over sideways.

On flat field tiles, yaw is the angle you care about most. In the FTC SDK, yaw is reported from -180 to +180 degrees. Turning counterclockwise (to the left, seen from above) makes yaw increase. Calling resetYaw() makes the current direction count as 0 degrees, so you usually do that during INIT once the robot is placed on its start position.

Telling the SDK how the hub is mounted

The IMU only knows its own axes. To report the robot's yaw, the SDK needs to know how the Control Hub sits on the robot. RevHubOrientationOnRobot describes this with two directions: which way the REV logo on the hub faces, and which way the USB ports face. For a hub lying flat with the logo facing up and the USB ports facing the front of the robot, it looks like this (if your hub is mounted on its side or upside down, choose the matching values, such as LogoFacingDirection.LEFT or UsbFacingDirection.UP; the two directions must always be at right angles to each other):

1// Imports needed:
2// com.qualcomm.hardware.rev.RevHubOrientationOnRobot
3// com.qualcomm.robotcore.hardware.IMU
4// org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
5// org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles
6
7IMU imu = hardwareMap.get(IMU.class, "imu");
8RevHubOrientationOnRobot orientation = new RevHubOrientationOnRobot(
9        RevHubOrientationOnRobot.LogoFacingDirection.UP,
10        RevHubOrientationOnRobot.UsbFacingDirection.FORWARD);
11imu.initialize(new IMU.Parameters(orientation));
12imu.resetYaw(); // the current direction becomes 0 degrees
13
14waitForStart();
15
16while (opModeIsActive()) {
17    YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
18    telemetry.addData("Yaw (deg)", "%.1f", angles.getYaw(AngleUnit.DEGREES));
19    telemetry.addData("Pitch (deg)", "%.1f", angles.getPitch(AngleUnit.DEGREES));
20    telemetry.addData("Roll (deg)", "%.1f", angles.getRoll(AngleUnit.DEGREES));
21    telemetry.update();
22}

Turning to a heading

The simplest turn spins the robot at a low, constant power until the heading reaches the target. The helper below turns left: the left wheels go backward and the right wheels go forward. Note that it switches the motors to RUN_USING_ENCODER first, because after a RUN_TO_POSITION drive the motors would otherwise keep trying to hold their old target.

1private double getHeading() {
2    return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
3}
4
5private void turnLeftTo(double targetDeg, double power) {
6    leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
7    rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
8    while (opModeIsActive() && getHeading() < targetDeg - 2.0) {
9        leftDrive.setPower(-power);
10        rightDrive.setPower(power);
11        telemetry.addData("Heading", "%.1f", getHeading());
12        telemetry.update();
13    }
14    leftDrive.setPower(0);
15    rightDrive.setPower(0);
16}

This works at low power, but at higher power the robot overshoots because it keeps spinning after the motors stop. It also has a hidden trap: angle wrap-around. If you are at 179 degrees and turn 2 degrees more to the left, yaw jumps to -179, not 181. The real difference is only 2 degrees, not 358. In the next lesson you will fix both problems with a proportional controller and AngleUnit.normalizeDegrees(), which converts any angle difference into the range -180 to +180.