Building, Testing and Debugging a Routine

You now have all the pieces: encoders for distance, the IMU for heading, and P-control for accuracy. This lesson puts them together into a complete routine and shows you how to prove it works.

1. 1
   Fix the start position

   Decide exactly where the robot starts and how you will line it up, for example with its back against the wall and its side on a tile seam. Mark it with tape.
2. 2
   List the moves

   Write each move as a line: drive 24 inches, turn to 90 degrees, drive 18 inches. Field tiles are 24 inches, which makes measuring easy.
3. 3
   Choose where to end

   Pick a safe parking zone that does not block your alliance partner, and agree on it with them.
4. 4
   Set a time budget

   Aim to finish well under 30 seconds, for example around 20, so slow runs still finish in time.

Structure with helper methods

Keep runOpMode() short so it reads like your list of moves. All the details live in helper methods: initHardware(), driveInches(), turnToHeading() and getHeading() from earlier lessons. Each helper has its own timeout, and one ElapsedTime called runtime measures the whole routine.

1// Imports as in earlier lessons, plus com.qualcomm.robotcore.util.RobotLog
2@Autonomous(name = "Basic Auto", group = "Auto")
3public class BasicAuto extends LinearOpMode {
4    static final double TICKS_PER_INCH = 45.3; // your calibrated value
5    private DcMotor leftDrive, rightDrive;
6    private IMU imu;
7    private final ElapsedTime runtime = new ElapsedTime();
8
9    @Override
10    public void runOpMode() {
11        initHardware(); // map motors, set directions, init IMU, resetYaw()
12        telemetry.addData("Status", "Ready, heading %.1f", getHeading());
13        telemetry.update();
14        waitForStart();
15        runtime.reset();
16
17        step("Drive to first mark");
18        driveInches(24, 0.5, 4.0);
19        step("Turn left to 90");
20        turnToHeading(90);
21        step("Drive to parking zone");
22        driveInches(18, 0.5, 4.0);
23        step("Done");
24    }
25
26    private void step(String name) {
27        telemetry.addData("Step", name);
28        telemetry.addData("Time", "%.1f s", runtime.seconds());
29        telemetry.update();
30        RobotLog.d("AUTO %s at %.1f s", name, runtime.seconds());
31    }
32
33    // initHardware(), driveInches(), turnToHeading(), getHeading() go here
34}

Debugging with telemetry and logs

• Show the current step name, so you know exactly where the routine stopped or went wrong.
• Show target and actual values: target ticks and encoder counts, target heading and yaw.
• Show elapsed time and battery voltage, so slow runs can be explained.
• While debugging, add sleep(500) between steps to slow things down and read the screen.
• Use RobotLog.d() to write messages to the robot log (logcat), which you can read later in Android Studio even after telemetry has changed.

Repeatability testing

One good run proves very little. Run the full routine at least 10 times in a row from the same start position and measure where the robot stops each time. If the robot always misses in the same direction by about the same amount, that is a consistent error, and you can fix it by calibration. If it lands in a different place each time, look for random causes such as wheel slip, loose parts or careless start alignment.