Week 10 · Lesson 2 60 min

Final Programming and Autonomous Testing

Combine drive and mechanism controls into one TeleOp, write a simple Autonomous, and prove it is repeatable from every start position.

In this lesson you will

Design a control map that splits tasks between two drivers
Write one TeleOp that controls the drivetrain and the mechanisms
Write a simple encoder-based Autonomous with safety timeouts
Run and log repeatability tests from each start position

Lesson

Your final code has two jobs. The TeleOp must give drivers smooth control of the whole robot, and the Autonomous must do something simple, the same way, every time. For a new team, a reliable Autonomous that scores a few points is worth more than an ambitious one that works once in five tries. Near the end of the season, also agree on a code freeze: a point after which you only fix bugs and stop adding features. Save a working copy of your code before every change, so you can always go back to the last version that worked if something breaks the night before an event.

Combining TeleOp controls

Most teams split the controls between two people: the driver on gamepad 1 moves the robot, and the operator on gamepad 2 runs the mechanisms. This keeps each person focused on one task. Before you write any code, agree on a control map with your drivers and write it down, so everyone knows which button does what.

Control	Gamepad	Action
Left stick up/down	Gamepad 1	Drive forward and backward
Right stick left/right	Gamepad 1	Turn
Y / A buttons	Gamepad 2	Arm to UP / DOWN preset
Right bumper / left bumper	Gamepad 2	Close / open claw

JAVA

1@TeleOp(name = "Final TeleOp")
2public class FinalTeleOp extends LinearOpMode {
3    @Override
4    public void runOpMode() {
5        DcMotor leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
6        DcMotor rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
7        rightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
8        Servo claw = hardwareMap.get(Servo.class, "claw");
9        Arm arm = new Arm(hardwareMap);
10
11        waitForStart();
12        while (opModeIsActive()) {
13            // Gamepad 1: drive
14            double drive = -gamepad1.left_stick_y;
15            double turn = gamepad1.right_stick_x;
16            leftDrive.setPower(Range.clip(drive + turn, -1, 1));
17            rightDrive.setPower(Range.clip(drive - turn, -1, 1));
18
19            // Gamepad 2: mechanisms
20            if (gamepad2.y) arm.goTo(Arm.UP);
21            if (gamepad2.a) arm.goTo(Arm.DOWN);
22            if (gamepad2.right_bumper) claw.setPosition(0.7); // close
23            if (gamepad2.left_bumper) claw.setPosition(0.2);  // open
24
25            telemetry.addData("Arm ticks", arm.getPosition());
26            telemetry.update();
27        }
28    }
29}

One TeleOp for drive and mechanisms, using the Arm subsystem from the previous lesson.

A simple Autonomous

A match starts with a 30-second Autonomous period, then an 8-second transition, then 2 minutes of Driver-Controlled. Check the current manual for exact timing.

During Autonomous the robot runs on its own code only. A good first goal is to leave the starting area and park, or to deliver one preloaded game element and then park. How many points each action is worth changes every season, so check the current game manual to decide what is worth doing. The example below uses encoders with RUN_TO_POSITION and a timeout, so the robot cannot get stuck forever if a wheel slips.

JAVA

1@Autonomous(name = "Simple Auto")
2public class SimpleAuto extends LinearOpMode {
3    private DcMotor leftDrive, rightDrive;
4
5    @Override
6    public void runOpMode() {
7        leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
8        rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
9        rightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
10        waitForStart();
11
12        driveTicks(1000, 0.4);  // forward; measure the distance on your robot
13        sleep(250);
14        driveTicks(-300, 0.4);  // back up into the parking spot
15    }
16
17    private void driveTicks(int ticks, double power) {
18        leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
19        rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
20        leftDrive.setTargetPosition(ticks);
21        rightDrive.setTargetPosition(ticks);
22        leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
23        rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
24        leftDrive.setPower(power);
25        rightDrive.setPower(power);
26        ElapsedTime timer = new ElapsedTime();
27        while (opModeIsActive() && leftDrive.isBusy() && rightDrive.isBusy()
28                && timer.seconds() < 4) {
29            telemetry.addData("Left ticks", leftDrive.getCurrentPosition());
30            telemetry.update();
31        }
32        leftDrive.setPower(0);
33        rightDrive.setPower(0);
34    }
35}

A simple encoder-based Autonomous: drive forward, then back up to park.

Repeatability testing

One good run proves very little. Repeatability means the robot gets the same result run after run. You also need to test from every start position you might be given: usually a left and a right side for both the red and the blue alliance. A path that works on one side often needs to be mirrored, with turns reversed, for the other. Test on the same kind of tiles you will compete on if you can, because worn or dusty tiles change how much the wheels grip, and that can shift where the robot stops.

1
Mark the start
Put painter's tape on the tile to show exactly where the robot starts. Line the robot up against the wall or tape the same way every run.
2
Run it at least 10 times
Run the Autonomous at least 10 times from each start position, with a charged battery.
3
Record every result
Note success or failure and measure how far the robot ended from the target, even on good runs.
4
Change one thing at a time
If results are inconsistent, change a single value, such as power or a target, then test again. Changing several things at once hides the real cause.

Run	Start position	Result	Distance from target	Notes
1	Red, left	Parked / missed	measure in cm	battery voltage, anything unusual
2	Red, left
...	Blue, right

Key terms

Control map: A written plan showing which gamepad button or stick controls each robot action.
Preset: A saved mechanism position, such as arm UP, that a single button press moves to.
Repeatability: The ability of the robot to produce the same result every time it runs the same code from the same start.
Start position: The marked place on the field where the robot is set up before a match begins.
Timeout: A time limit in a loop that stops the action if the target is not reached, so the robot never gets stuck.

Practical exercise

Build and prove your match code

1Write your team's control map in a table and agree on it with both drivers.
2Combine your drive and mechanism code into one TeleOp and test every control.
3Write a simple Autonomous with encoder moves and a timeout in every loop.
4Mark each start position with tape and run the Autonomous at least 10 times from each one.
5Log every run in a table and calculate how many runs succeeded from each position.

Deliverable: The TeleOp and Autonomous code files, your control map, and a test log with at least 10 runs per start position and a success count for each.

Further resources

Mark lesson complete