TeleOp Programming Exercises and Challenges

intermediate150 min

Purpose of Exercises

Hands-on practice is essential for mastering TeleOp programming. These exercises are designed to reinforce and apply the concepts you’ve learned in previous lessons.

Drive in a Square (with REV IMU)

Write code to make the robot drive in a square pattern using TeleOp controls. Use the built-in REV IMU to turn exactly 90 degrees for each corner. Use helper methods for each movement.

Map drive motors and the built-in REV IMU.
Write a helper method to drive forward for a set time.
Write a helper method to turn 90 degrees using the IMU.
Use a button to trigger the square-driving sequence.

Map drive motors and the built-in REV IMU.

                  // Map drive motors and IMU in your OpMode
DcMotor leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
DcMotor rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
IMU imu = hardwareMap.get(IMU.class, "imu");
// Make sure these names match your robot configuration!

Write a helper method to drive forward for a set time.

                  // Helper method to drive forward
private void driveForward(double power, long timeMs) {
    leftDrive.setPower(power);
    rightDrive.setPower(power);
    sleep(timeMs);
    leftDrive.setPower(0);
    rightDrive.setPower(0);
}

Write a helper method to turn 90 degrees using the IMU.

                  // Helper method to turn 90 degrees (right) using the IMU
private void turn90WithIMU(double power) {
    double startAngle = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
    double targetAngle = startAngle + 90.0;
    // Normalize target angle to [-180, 180]
    if (targetAngle > 180) targetAngle -= 360;
    leftDrive.setPower(power);
    rightDrive.setPower(-power);
    while (opModeIsActive() && Math.abs(getAngleDiff(
        imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES), targetAngle)) > 2.0) {
        telemetry.addData("Turning to", targetAngle);
        telemetry.addData("Current", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
        telemetry.update();
    }
    leftDrive.setPower(0);
    rightDrive.setPower(0);
}
// Helper to compute shortest angle difference
private double getAngleDiff(double current, double target) {
    double diff = target - current;
    while (diff > 180) diff -= 360;
    while (diff < -180) diff += 360;
    return diff;
}
// Adjust the power and tolerance as needed for your robot.

Use a button to trigger the square-driving sequence.

                  // In your main loop
if (gamepad1.x && !lastX) {
    for (int i = 0; i < 4; i++) {
        driveForward(0.5, 1000); // Drive forward
        turn90WithIMU(0.3);      // Turn 90 degrees using IMU
    }
}
// Make sure to debounce the button or only allow the sequence to run once per press.

Combine all tasks into one working OpMode.

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

@TeleOp(name = "DriveSquareIMUOpMode")
public class DriveSquareIMUOpMode extends LinearOpMode {
    private DcMotor leftDrive, rightDrive;
    private IMU imu;
    private boolean lastX = false;
    @Override
    public void runOpMode() {
        leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
        rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
        imu = hardwareMap.get(IMU.class, "imu");
        // Set up IMU orientation (adjust for your robot's mounting)
        RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(
            LogoFacingDirection.UP, UsbFacingDirection.FORWARD);
        imu.initialize(new IMU.Parameters(orientationOnRobot));
        waitForStart();
        while (opModeIsActive()) {
            if (gamepad1.x && !lastX) {
                for (int i = 0; i < 4; i++) {
                    driveForward(0.5, 1000);
                    turn90WithIMU(0.3);
                }
            }
            lastX = gamepad1.x;
            telemetry.addData("Status", "Waiting for X button");
            telemetry.update();
        }
    }
    private void driveForward(double power, long timeMs) {
        leftDrive.setPower(power);
        rightDrive.setPower(power);
        sleep(timeMs);
        leftDrive.setPower(0);
        rightDrive.setPower(0);
    }
    private void turn90WithIMU(double power) {
        double startAngle = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
        double targetAngle = startAngle + 90.0;
        if (targetAngle > 180) targetAngle -= 360;
        leftDrive.setPower(power);
        rightDrive.setPower(-power);
        while (opModeIsActive() && Math.abs(getAngleDiff(
            imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES), targetAngle)) > 2.0) {
            telemetry.addData("Turning to", targetAngle);
            telemetry.addData("Current", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
            telemetry.update();
        }
        leftDrive.setPower(0);
        rightDrive.setPower(0);
    }
    private double getAngleDiff(double current, double target) {
        double diff = target - current;
        while (diff > 180) diff -= 360;
        while (diff < -180) diff += 360;
        return diff;
    }
}

Servo Toggle

Implement a button toggle to open/close a claw or move a servo to two positions.

Map a servo.
Use edge detection to toggle the servo position with a button.
Display the servo state with telemetry.

Map a servo.

                  // Map the servo
Servo clawServo = hardwareMap.get(Servo.class, "claw_servo");

Use edge detection to toggle the servo position with a button.

                  // Edge detection for toggle
private boolean lastA = false;
private boolean clawOpen = false;
private void toggleClaw() {
    if (gamepad1.a && !lastA) {
        clawOpen = !clawOpen;
        clawServo.setPosition(clawOpen ? 1.0 : 0.0);
    }
    lastA = gamepad1.a;
}

Display the servo state with telemetry.

                  // Telemetry for servo state
telemetry.addData("Claw State", clawOpen ? "Open" : "Closed");
telemetry.update();

Combine all tasks into one working OpMode.

                  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;

@TeleOp(name = "ServoToggleOpMode")
public class ServoToggleOpMode extends LinearOpMode {
    private Servo clawServo;
    private boolean lastA = false;
    private boolean clawOpen = false;
    @Override
    public void runOpMode() {
        clawServo = hardwareMap.get(Servo.class, "claw_servo");
        waitForStart();
        while (opModeIsActive()) {
            if (gamepad1.a && !lastA) {
                clawOpen = !clawOpen;
                clawServo.setPosition(clawOpen ? 1.0 : 0.0);
            }
            lastA = gamepad1.a;
            telemetry.addData("Claw State", clawOpen ? "Open" : "Closed");
            telemetry.update();
        }
    }
}

Display Arm Position

Use telemetry to show the current position of an arm or lift.

Map an arm or lift motor with an encoder.
Read the encoder value.
Display the value with telemetry.

Map an arm or lift motor with an encoder.

                  // Map the arm/lift motor
DcMotor armMotor = hardwareMap.get(DcMotor.class, "arm_motor");

Read the encoder value.

                  // Read encoder value
int armPosition = armMotor.getCurrentPosition();

Display the value with telemetry.

                  // Telemetry for arm position
telemetry.addData("Arm Position", armMotor.getCurrentPosition());
telemetry.update();

Combine all tasks into one working OpMode.

                  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;

@TeleOp(name = "ArmPositionOpMode")
public class ArmPositionOpMode extends LinearOpMode {
    private DcMotor armMotor;
    @Override
    public void runOpMode() {
        armMotor = hardwareMap.get(DcMotor.class, "arm_motor");
        waitForStart();
        while (opModeIsActive()) {
            int armPosition = armMotor.getCurrentPosition();
            telemetry.addData("Arm Position", armPosition);
            telemetry.update();
        }
    }
}

Slow Mode

Add a button to enable/disable slow mode for precise driving.

Use a button to toggle slow mode.
Multiply drive power by a lower value when slow mode is active.
Display slow mode status with telemetry.

Use a button to toggle slow mode.

                  // Use left bumper for slow mode
boolean slowMode = gamepad1.left_bumper;

Multiply drive power by a lower value when slow mode is active.

                  // Apply speed scaling
double speed = slowMode ? 0.3 : 1.0;
leftDrive.setPower(-gamepad1.left_stick_y * speed);
rightDrive.setPower(-gamepad1.right_stick_y * speed);

Display slow mode status with telemetry.

                  // Telemetry for slow mode
telemetry.addData("Mode", slowMode ? "SLOW" : "NORMAL");
telemetry.update();

Combine all tasks into one working OpMode.

                  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;

@TeleOp(name = "SlowModeOpMode")
public class SlowModeOpMode extends LinearOpMode {
    private DcMotor leftDrive, rightDrive;
    @Override
    public void runOpMode() {
        leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
        rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
        waitForStart();
        while (opModeIsActive()) {
            boolean slowMode = gamepad1.left_bumper;
            double speed = slowMode ? 0.3 : 1.0;
            leftDrive.setPower(-gamepad1.left_stick_y * speed);
            rightDrive.setPower(-gamepad1.right_stick_y * speed);
            telemetry.addData("Mode", slowMode ? "SLOW" : "NORMAL");
            telemetry.update();
        }
    }
}

Multi-Subsystem Control

Control two mechanisms (e.g., drive and arm) simultaneously with different gamepad inputs.

Map drive motors and an arm motor.
Use the left stick for driving and buttons for the arm.
Add telemetry for both subsystems.

Map drive motors and an arm motor.

                  // Map hardware
DcMotor leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
DcMotor rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
DcMotor armMotor = hardwareMap.get(DcMotor.class, "arm_motor");

Use the left stick for driving and buttons for the arm.

                  // Drive and arm control
leftDrive.setPower(-gamepad1.left_stick_y);
rightDrive.setPower(-gamepad1.right_stick_y);
if (gamepad1.a) {
    armMotor.setPower(1.0);
} else if (gamepad1.b) {
    armMotor.setPower(-1.0);
} else {
    armMotor.setPower(0.0);
}

Add telemetry for both subsystems.

                  // Telemetry for drive and arm
telemetry.addData("Left Power", leftDrive.getPower());
telemetry.addData("Right Power", rightDrive.getPower());
telemetry.addData("Arm Position", armMotor.getCurrentPosition());
telemetry.update();

Combine all tasks into one working OpMode.

                  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;

@TeleOp(name = "MultiSubsystemOpMode")
public class MultiSubsystemOpMode extends LinearOpMode {
    private DcMotor leftDrive, rightDrive, armMotor;
    @Override
    public void runOpMode() {
        leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
        rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
        armMotor = hardwareMap.get(DcMotor.class, "arm_motor");
        waitForStart();
        while (opModeIsActive()) {
            leftDrive.setPower(-gamepad1.left_stick_y);
            rightDrive.setPower(-gamepad1.right_stick_y);
            if (gamepad1.a) {
                armMotor.setPower(1.0);
            } else if (gamepad1.b) {
                armMotor.setPower(-1.0);
            } else {
                armMotor.setPower(0.0);
            }
            telemetry.addData("Left Power", leftDrive.getPower());
            telemetry.addData("Right Power", rightDrive.getPower());
            telemetry.addData("Arm Position", armMotor.getCurrentPosition());
            telemetry.update();
        }
    }
}

Troubleshooting Scenarios

Practice diagnosing and fixing common TeleOp issues.

Fix code where a motor does not move.
Debug a button that does not trigger the expected action.
Find and fix a missing telemetry.update() call.

Fix code where a motor does not move.

                  // Check hardware mapping and power
// Example: Make sure the name matches configuration
DcMotor leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
leftDrive.setPower(0.5); // Set a nonzero power
// Also check for wiring and configuration issues.

Debug a button that does not trigger the expected action.

                  // Check for correct button logic
if (gamepad1.a) {
    // Action here
}
// Make sure the button is not being overridden elsewhere and that the gamepad is connected.

Find and fix a missing telemetry.update() call.

                  // Always call telemetry.update() after adding data
telemetry.addData("Status", "Running");
telemetry.update(); // This line is required to display telemetry on the Driver Station.

Combine all tasks into one working OpMode.

                  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;

@TeleOp(name = "TroubleshootingOpMode")
public class TroubleshootingOpMode extends LinearOpMode {
    private DcMotor leftDrive;
    @Override
    public void runOpMode() {
        leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
        waitForStart();
        while (opModeIsActive()) {
            // Example: Fix motor not moving
            leftDrive.setPower(0.5);
            // Example: Button debug
            if (gamepad1.a) {
                telemetry.addData("Button A", "Pressed");
            }
            // Example: Telemetry update
            telemetry.addData("Status", "Running");
            telemetry.update();
        }
    }
}

Reflection and Extension

After completing these exercises, think about how you could extend your code. Try adding a new feature, optimizing your logic, or combining multiple exercises into a single OpMode.

TeleOp Programming Exercises and Challenges

Purpose of Exercises

Drive in a Square (with REV IMU)

Servo Toggle

Display Arm Position

Slow Mode

Multi-Subsystem Control

Troubleshooting Scenarios

Reflection and Extension

Related Topics and Documentation