Lesson 12.5: Challenge — Implementing an IMU-Corrected 90-Degree Autonomous Turn
Why Time-Based Turns Fail at Competition
The simplest autonomous turn is a timed one: apply power to the motors for a fixed number of milliseconds and assume the robot reached the target angle. This works acceptably on a freshly charged battery with a clean, consistent floor surface in your practice space. It fails for three reasons that compound over the course of a competition day.
First, the battery voltage drops with each match, so the same sleep(500) call produces a progressively smaller angle as the day goes on. Second, the FTC field tiles have a slight texture variation from panel to panel, and some panels grip the wheels more than others. Third, any contact with another robot or a field element during the turn changes the outcome unpredictably. A time-based turn has no way to detect or correct for any of these factors.
An IMU-corrected turn closes the loop. The robot reads its actual heading each loop cycle, compares it to the target, and adjusts motor power accordingly. It stops when the sensor confirms it has reached the target, not when a timer expires. The result is a turn that is accurate on low batteries, on varying floor surfaces, and after incidental contact.
Proportional Control: Scaling Power to Error
A naive IMU turn implementation applies full power until the heading is within a small tolerance of the target. This overshoots badly because the robot is moving at full speed when it crosses the threshold and continues rotating due to inertia before the motors can respond.
The correct approach is proportional control, often abbreviated as P-control. The motor power is set to a fraction of the remaining error:
double error = targetAngle - currentAngle;
double power = Range.clip(error * kP, -maxPower, maxPower);
Where kP is a small tuning constant called the proportional gain. When the error is large (far from the target), the power is larger. As the error shrinks on approach, the power scales down automatically. The Range.clip() call from the FTC SDK ensures the calculated power never exceeds a safe maximum, regardless of the error magnitude.
One additional subtlety: heading error can wrap around. If the robot is at 170 degrees and the target is -170 degrees, the naive calculation gives an error of -340 degrees, causing the robot to turn the long way around. The AngleUnit.normalizeRadians() or equivalent degree normalization logic must be applied to the error so the robot always takes the shorter path.
Annotated Code
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
@Autonomous(name="IMU_Turn_Challenge")
public class ImuTurnChallenge extends LinearOpMode {
private DcMotor leftDrive, rightDrive;
private IMU imu;
// Tuning constants -- adjust kP and tolerance during practice
private static final double kP = 0.012; // proportional gain
private static final double MAX_POWER = 0.5; // never exceed this during a turn
private static final double TOLERANCE = 1.5; // degrees -- acceptable error at stop
@Override
public void runOpMode() {
leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
imu = hardwareMap.get(IMU.class, "imu");
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
)));
waitForStart();
// Turn to exactly 90 degrees to the right (positive yaw)
turnToAngle(90.0);
// After the turn, the robot is confirmed at the target heading
telemetry.addData("Turn", "Complete");
telemetry.update();
sleep(1000);
}
/**
* Rotates the robot to the specified heading in degrees using proportional
* control. Blocks until the heading is within TOLERANCE of the target
* or the OpMode is stopped.
*/
private void turnToAngle(double targetDegrees) {
while (opModeIsActive()) {
double currentAngle = imu.getRobotYawPitchRollAngles()
.getYaw(AngleUnit.DEGREES);
double error = targetDegrees - currentAngle;
// Normalize error to -180..180 so the robot takes the shorter arc
while (error > 180.0) error -= 360.0;
while (error < -180.0) error += 360.0;
// Exit when the error is small enough
if (Math.abs(error) < TOLERANCE) {
leftDrive.setPower(0.0);
rightDrive.setPower(0.0);
break;
}
// Proportional power: larger error = more power, smaller error = less power
double power = Range.clip(error * kP, -MAX_POWER, MAX_POWER);
// Positive error turns left (counterclockwise); motors driven accordingly
leftDrive.setPower(-power);
rightDrive.setPower(power);
telemetry.addData("Target (deg)", targetDegrees);
telemetry.addData("Current (deg)", "%.2f", currentAngle);
telemetry.addData("Error (deg)", "%.2f", error);
telemetry.addData("Power", "%.3f", power);
telemetry.update();
}
}
}
Fill-in-the-Blank Practice
- The difference between the target heading and the robot's current heading is called the
__________. - A control strategy where motor power is proportional to the remaining error, scaling down automatically as the robot approaches its target, is called
__________control. - To prevent the robot from taking the long path around a heading boundary (for example, turning 340 degrees instead of 20 degrees), the heading error must be
__________to the -180 to 180 range.
Show answers
- error
- proportional (P-control)
- normalized
Template Challenge
Robot Scenario: Complete the autonomous turn sequence below. The robot should turn to 90 degrees using the proportional control pattern, then drive forward at 0.4 power for 1.5 seconds once the turn is confirmed complete. Use the structure provided and fill in only the missing sections.
@Autonomous(name="Turn_Then_Drive_Challenge")
public class TurnThenDrive extends LinearOpMode {
private DcMotor leftDrive, rightDrive;
private IMU imu;
private static final double kP = 0.012;
private static final double MAX_POWER = 0.5;
private static final double TOLERANCE = 1.5;
@Override
public void runOpMode() {
leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
imu = hardwareMap.get(IMU.class, "imu");
// INSERT CODE HERE: Initialize the IMU with logo UP and USB FORWARD
waitForStart();
// INSERT CODE HERE: Write the proportional turn loop targeting 90 degrees.
// Exit the loop when Math.abs(error) < TOLERANCE.
// Apply -power to leftDrive and +power to rightDrive.
// INSERT CODE HERE: After the turn exits, drive forward at 0.4 power
// for 1.5 seconds using a non-blocking timer (getRuntime()).
// Then stop both motors.
}
}
Show answer
// IMU initialization
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
)));
// Proportional turn to 90 degrees
double targetDegrees = 90.0;
while (opModeIsActive()) {
double currentAngle = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
double error = targetDegrees - currentAngle;
while (error > 180.0) error -= 360.0;
while (error < -180.0) error += 360.0;
if (Math.abs(error) < TOLERANCE) {
leftDrive.setPower(0.0);
rightDrive.setPower(0.0);
break;
}
double power = Range.clip(error * kP, -MAX_POWER, MAX_POWER);
leftDrive.setPower(-power);
rightDrive.setPower(power);
}
// Drive forward for 1.5 seconds
double driveEnd = getRuntime() + 1.5;
while (opModeIsActive() && getRuntime() < driveEnd) {
leftDrive.setPower(0.4);
rightDrive.setPower(0.4);
}
leftDrive.setPower(0.0);
rightDrive.setPower(0.0);
Ready to move on?
Sign in with Google to save your progress with Telemark, or continue without saving.