Accuracy and Control Logic

Even with encoders and an IMU, a robot can still end up an inch or two away from where you wanted. That is normal. The goal is to understand where the error comes from, so you can make it small and, most importantly, consistent. A robot that always stops 1 inch short is easy to fix; a robot that stops anywhere within 6 inches of the target is not.

Why robots miss

Open loop and closed loop

Open-loop control means sending power and hoping: the timed drive from the first lesson is open loop. Closed-loop control measures the result, compares it with the target and corrects. RUN_TO_POSITION is closed loop inside the motor controller. Now you will build your own closed loop for turning.

Proportional control

A proportional controller (P-controller) sets power in proportion to the error: power = kP * error, where error is the target heading minus the current heading. With kP = 0.02, an error of 90 degrees gives 1.8, which we clip to a maximum of 0.5. An error of 10 degrees gives 0.2, and an error of 2 degrees gives only 0.04. So the robot turns fast when it is far away and slows down as it gets close, which greatly reduces overshoot.

1// Extra import: com.qualcomm.robotcore.util.Range
2static final double TURN_KP = 0.02;
3static final double TURN_TOLERANCE_DEG = 2.0;
4
5private void turnToHeading(double targetDeg) {
6    leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
7    rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
8    ElapsedTime timer = new ElapsedTime();
9
10    while (opModeIsActive() && timer.seconds() < 3.0) {
11        double error = AngleUnit.normalizeDegrees(targetDeg - getHeading());
12        if (Math.abs(error) < TURN_TOLERANCE_DEG) {
13            break;
14        }
15        // positive error means turn left (counterclockwise)
16        double power = Range.clip(TURN_KP * error, -0.5, 0.5);
17        leftDrive.setPower(-power);
18        rightDrive.setPower(power);
19        telemetry.addData("Error (deg)", "%.1f", error);
20        telemetry.addData("Power", "%.2f", power);
21        telemetry.update();
22    }
23    leftDrive.setPower(0);
24    rightDrive.setPower(0);
25}

Sequential code and state machines

So far each helper blocks: driveInches() does not return until the move is finished. This sequential style reads like a recipe and is perfect for a first routine. Its limit is that the robot can only do one thing at a time. A state machine keeps track of which named step (state) the robot is in. Each time through the loop it checks a condition and moves to the next state when that condition is met, so several mechanisms can move at once.

1// Declared inside the class, outside runOpMode():
2enum State { DRIVE_AND_LIFT, BACK_UP, DONE }
3
4// Inside runOpMode(), after waitForStart():
5State state = State.DRIVE_AND_LIFT;
6ElapsedTime stateTimer = new ElapsedTime();
7startDrive(24);       // sets targets and RUN_TO_POSITION, returns at once
8startArm(ARM_UP);     // the arm moves while the robot drives
9
10while (opModeIsActive() && state != State.DONE) {
11    switch (state) {
12        case DRIVE_AND_LIFT:
13            if ((!driveBusy() && !arm.isBusy()) || stateTimer.seconds() > 4.0) {
14                startDrive(-6);
15                stateTimer.reset();
16                state = State.BACK_UP;
17            }
18            break;
19        case BACK_UP:
20            if (!driveBusy() || stateTimer.seconds() > 2.0) {
21                state = State.DONE;
22            }
23            break;
24        default:
25            break;
26    }
27    telemetry.addData("State", state);
28    telemetry.update();
29}

Which should you use? Start with sequential code; it is easier to read and debug. Move to a state machine when you need actions to overlap, or when the robot must react to sensors at any moment. Either way, every step needs a timeout so one failed step cannot freeze the whole routine. Writing these simple versions yourself first is the best way to understand what more advanced control tools do later.