PID control

Imagine your arm must lift to 60°. A timed command might run its motor for half a second without checking the final angle. The arm can stop short when the battery is low, or travel too far when the arm is lighter. A PID controller checks the arm's encoder while it moves and adjusts motor power based on what the arm actually did.

This lesson builds that controller in stages. You will first check the encoder and motor direction, then add proportional control, and only then add derivative and integral correction. The example is for a single encoder-equipped arm; your gear ratio, arm range, motor direction, and gains must be measured on your robot.

Prerequisites

You need:

• A motor with an encoder attached to the arm.
• The motor configured in the Robot Controller configuration. This example calls it arm.
• A known safe low and high arm position, with hard stops or another way to keep the arm inside its real travel range.
• A way to measure the arm angle, such as a printed angle guide or protractor.

Make the mechanism safe before testing. Support the arm so it cannot fall, keep hands clear of gears and pinch points, and begin with a small power limit. If motor power moves the arm away from the target, stop the OpMode and correct the motor direction before continuing.

Feedback

A timed motor command is open-loop: you choose a power and duration without measuring the result. Battery voltage, friction, and load can change the final angle. A feedback controller measures and corrects that angle repeatedly:

1. Read the encoder and convert its counts to the arm's angle.
2. Compare that measured angle with the target.
3. Turn the difference into a motor-power command.
4. Apply the command briefly, then measure again on the next loop.

The target is the setpoint, the encoder-derived angle is the measurement, and their difference is the error:

error = target angle - measured angle

If the target is 60° and the encoder says 20°, the error is 60 - 20 = 40°. Positive error means the arm needs to move toward a larger angle. Your motor's configured direction determines which power sign produces that motion, so verify the sign at low power before tuning.

The letters describe three ways of responding to that error. A common continuous-time form is:

You can skip the equation on a first read. The table below gives the practical meaning of each part.

Here e is the error and u is the requested motor command. The controller below runs digitally, so it approximates the integral by adding error × elapsed time on each loop. It estimates speed from the change in measured angle divided by elapsed time. The gains are tied to these units: with angle in degrees and output in motor power, kP has units of power per degree; kI has units of power per degree-second; kD has units of power-seconds per degree. Changing the sensor units or time units means the gains must change too.

P, I, and D

• P — Proportional: reacts to the gap right now. A bigger gap asks for a stronger push. Start here; too much P can make the arm overshoot.
• I — Integral: reacts to an error that keeps happening. It can help when the arm repeatedly stops a little short, but stored error can build up while the motor is blocked. Leave I at zero until P and D behave well.
• D — Derivative: reacts to how fast the arm is moving. It can brake an arm approaching its target, but too much D can make the motor chatter.

The finished example limits stored I error and estimates D from measured arm speed. These choices avoid two common problems, but the gains and limits still need tuning on your robot.

The example combines the terms like this:

requested power = P contribution + I contribution + D contribution
               = kP × error + kI × accumulated error - kD × measured speed

The minus sign on the speed term makes it oppose the arm's current movement. The requested power is then limited to the configured maximum. Once that limit is reached, a bigger controller request cannot produce more motor power, which is why integral buildup needs a limit too.

PID does not mean all three gains must be nonzero. Start with P, add D if the arm overshoots, and try I only when repeatable tests show a small error that remains. An arm also has gravity acting on it; a PID loop may need a properly measured gravity-compensation term to hold a position without a large integral correction.

Term comparison

Encoder reference

An encoder reports counts, not degrees. To convert it into an angle, first choose what angle should mean zero. In this example, zero is the arm's safe lower position.

With the robot powered off, support the arm and place it at that reference mark. Then reset the encoder at initialization:

arm = hardwareMap.get(DcMotor.class, "arm");
        arm.setDirection(DcMotor.Direction.FORWARD); // Example: positive power must raise angle.
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
arm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

arm = hardwareMap.get(DcMotor::class.java, "arm")
        arm.direction = com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.FORWARD // Example: positive power must raise angle.
arm.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
arm.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
arm.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER

STOP_AND_RESET_ENCODER makes the current encoder reading zero. It does not move or home the arm. Only reset it when the arm is physically at the reference angle. RUN_WITHOUT_ENCODER lets our code calculate power from the reading on every loop.

Now measure the encoder's scale. Move the arm slowly between two measured angles and record the change in encoder counts. For example, if it changes by 336 counts while the arm moves 60°, then:

counts per degree = 336 counts ÷ 60 degrees = 5.6 counts/degree

Those numbers are an example measurement, not a motor specification. For this example, configure motor direction first, so a small positive logical power raises the arm's defined angle. Then measure the resulting positive counts per degree. The SDK reverses both logical motor power and encoder readings when direction changes, so redo calibration after a direction change. If an external sensor uses the opposite sign, convert its measurement and the controller's output polarity deliberately; changing only a sensor scale can turn negative feedback into positive feedback.

private static final double COUNTS_PER_DEGREE = 5.6; // Replace with your measurement

double currentAngle = arm.getCurrentPosition() / COUNTS_PER_DEGREE;

private val COUNTS_PER_DEGREE = 5.6 // Class field; replace with your measurement

val currentAngle = arm.currentPosition / COUNTS_PER_DEGREE

Check this before adding PID: manually move the arm through a known angle and compare the displayed angle with your protractor. If the sign is reversed or the scale is wrong, fix this conversion first.

Target and error

Use the D-pad to choose three test positions: 0°, 30°, or 60°. Keep targets inside the safe mechanical range of your arm.

if (gamepad1.dpad_down) targetAngle = 0.0;
if (gamepad1.dpad_left) targetAngle = 30.0;
if (gamepad1.dpad_up) targetAngle = 60.0;

double currentAngle = arm.getCurrentPosition() / COUNTS_PER_DEGREE;
double error = targetAngle - currentAngle;

if (gamepad1.dpad_down) targetAngle = 0.0
if (gamepad1.dpad_left) targetAngle = 30.0
if (gamepad1.dpad_up) targetAngle = 60.0

val currentAngle = arm.currentPosition / COUNTS_PER_DEGREE
val error = targetAngle - currentAngle

Before connecting motor power, send targetAngle, currentAngle, and error to telemetry. At the zero mark, telemetry should show a current angle close to 0°. When the arm is below a 60° target, the error should be positive. This tells you whether the sensor math agrees with the physical arm.

Proportional control

The proportional term is the error multiplied by kP:

double kP = 0.003; // A cautious starting example; tune for your mechanism

double power = kP * error;
power = Math.max(-0.25, Math.min(0.25, power)); // Low commissioning limit
arm.setPower(power);

val kP = 0.003 // A cautious starting example; tune for your mechanism

var power = kP * error
power = max(-0.25, min(0.25, power)) // Low commissioning limit
arm.power = power

Here kP is motor-power per degree. With a 40° error and kP = 0.003, the requested power is 0.12. The ±0.25 limit is just a cautious starting limit; do not assume it is safe or strong enough for every mechanism.

Test with the arm supported and watch closely. If positive error makes the arm move away from the target, stop, correct the motor direction or encoder sign, and test again. Increase kP in small steps only after the direction is correct.

What to look for:

• It barely moves and never gets close: raise kP a little.
• It reaches the target slowly without crossing it: P-only control may be good enough for this test.
• It swings past the target and comes back: keep the P value that moves it reliably, then continue to derivative control.

Do not copy a gain from another robot. Changing the arm length, gearing, load, encoder scale, or loop timing changes the behavior.

Derivative control

A fast-moving arm can pass the target even when the error is small. The derivative term measures the arm's speed and acts like a brake. This example calculates it from the measured angle, so choosing a new target does not create an artificial speed spike.

double degreesPerSecond = (currentAngle - previousAngle) / deltaTimeSeconds;
double dTerm = -kD * degreesPerSecond;

val degreesPerSecond = (currentAngle - previousAngle) / deltaTimeSeconds
val dTerm = -kD * degreesPerSecond

The minus sign makes the D term resist current motion. deltaTimeSeconds must be measured from a monotonic clock, such as System.nanoTime(), because loop timing can vary. The finished example below handles the first loop separately.

Start with kD = 0. After P moves the arm in the right direction, add a small D gain and change only that value at a time. If the arm slows smoothly near the target, D is helping. If it barely moves or jitters, reduce D; encoder noise can make derivative control noisy.

Integral control

If the arm repeatedly stops a few degrees short because of friction or load, the integral term can accumulate that leftover error:

integral += error * deltaTimeSeconds;
integral = Math.max(-MAX_INTEGRAL, Math.min(MAX_INTEGRAL, integral));
double iTerm = kI * integral;

integral += error * deltaTimeSeconds
integral = max(-MAX_INTEGRAL, min(MAX_INTEGRAL, integral))
val iTerm = kI * integral

The limit prevents the stored error from growing without bound while the arm is blocked or cannot reach its target. Keep kI at zero while you tune P and D. Add a very small value only if your measured tests show a steady offset. Too much I causes the arm to keep pushing after it reaches the target.

Complete controller

This complete Java example keeps the controller small enough to follow. The starting gains are deliberately modest examples, not guaranteed tuning values. Begin with KI and KD set to zero; tune on the real robot using the steps above. Change one gain at a time and keep the arm supported.

package org.firstinspires.ftc.teamcode;

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

@TeleOp(name = "Guided PID Arm")
public class GuidedPidArm extends LinearOpMode {
    // Configure direction first, then measure a positive counts/degree scale.
    private static final double COUNTS_PER_DEGREE = 5.6;
    private static final double KP = 0.003;
    private static final double KI = 0.0;
    private static final double KD = 0.0;
    private static final double MAX_POWER = 0.25;

    private DcMotor arm;
    private final ArmPid pid = new ArmPid(KP, KI, KD);

    @Override
    public void runOpMode() {
        arm = hardwareMap.get(DcMotor.class, "arm");
        arm.setDirection(DcMotor.Direction.FORWARD); // Example: positive power must raise angle.
        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        arm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

        double targetAngle = 0.0;
        telemetry.addLine("Check that the arm is at its zero reference.");
        telemetry.addLine("Press START only when the arm area is clear.");
        telemetry.update();
        waitForStart();
        pid.reset();

        try {
            while (opModeIsActive()) {
                if (gamepad1.dpad_down) targetAngle = 0.0;
                if (gamepad1.dpad_left) targetAngle = 30.0;
                if (gamepad1.dpad_up) targetAngle = 60.0;
                if (gamepad1.b) break;

                double currentAngle = arm.getCurrentPosition() / COUNTS_PER_DEGREE;
                double output = pid.calculate(targetAngle, currentAngle);
                double power = Math.max(-MAX_POWER, Math.min(MAX_POWER, output));
                arm.setPower(power);

                telemetry.addData("Target (deg)", "%.1f", targetAngle);
                telemetry.addData("Measured (deg)", "%.1f", currentAngle);
                telemetry.addData("Error (deg)", "%.1f", targetAngle - currentAngle);
                telemetry.addData("Motor power", "%.2f", power);
                telemetry.addLine("Press B to stop the test.");
                telemetry.update();
            }
        } finally {
            arm.setPower(0.0);
        }
    }

    private static class ArmPid {
        private static final double MAX_INTEGRAL = 300.0; // degree-seconds
        private final double kP;
        private final double kI;
        private final double kD;
        private double integral;
        private double previousAngle;
        private long previousTimeNanos;

        ArmPid(double kP, double kI, double kD) {
            this.kP = kP;
            this.kI = kI;
            this.kD = kD;
        }

        double calculate(double targetAngle, double currentAngle) {
            long now = System.nanoTime();
            double error = targetAngle - currentAngle;

            if (previousTimeNanos == 0L) {
                previousTimeNanos = now;
                previousAngle = currentAngle;
                return kP * error;
            }

            double dt = (now - previousTimeNanos) / 1_000_000_000.0;
            previousTimeNanos = now;
            if (dt <= 0.0 || dt > 0.1) {
                previousAngle = currentAngle;
                return kP * error + kI * integral;
            }

            integral += error * dt;
            integral = Math.max(-MAX_INTEGRAL, Math.min(MAX_INTEGRAL, integral));
            double degreesPerSecond = (currentAngle - previousAngle) / dt;
            previousAngle = currentAngle;

            return kP * error + kI * integral - kD * degreesPerSecond;
        }

        void reset() {
            integral = 0.0;
            previousAngle = 0.0;
            previousTimeNanos = 0L;
        }
    }
}

package org.firstinspires.ftc.teamcode

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
import com.qualcomm.robotcore.hardware.DcMotor
import kotlin.math.max
import kotlin.math.min

@TeleOp(name = "Guided PID Arm")
class GuidedPidArm : LinearOpMode() {
    companion object {
        // Configure direction first, then measure a positive counts/degree scale.
        private const val COUNTS_PER_DEGREE = 5.6
        private const val KP = 0.003
        private const val KI = 0.0
        private const val KD = 0.0
        private const val MAX_POWER = 0.25
    }

    private lateinit var arm: DcMotor
    private val pid = ArmPid(KP, KI, KD)

    override fun runOpMode() {
        arm = hardwareMap.get(DcMotor::class.java, "arm")
        arm.direction = com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.FORWARD // Example: positive power must raise angle.
        arm.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
        arm.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
        arm.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER

        var targetAngle = 0.0
        telemetry.addLine("Check that the arm is at its zero reference.")
        telemetry.addLine("Press START only when the arm area is clear.")
        telemetry.update()
        waitForStart()
        pid.reset()

        try {
            while (opModeIsActive()) {
                if (gamepad1.dpad_down) targetAngle = 0.0
                if (gamepad1.dpad_left) targetAngle = 30.0
                if (gamepad1.dpad_up) targetAngle = 60.0
                if (gamepad1.b) break

                val currentAngle = arm.currentPosition / COUNTS_PER_DEGREE
                val output = pid.calculate(targetAngle, currentAngle)
                val power = max(-MAX_POWER, min(MAX_POWER, output))
                arm.power = power

                telemetry.addData("Target (deg)", "%.1f", targetAngle)
                telemetry.addData("Measured (deg)", "%.1f", currentAngle)
                telemetry.addData("Error (deg)", "%.1f", targetAngle - currentAngle)
                telemetry.addData("Motor power", "%.2f", power)
                telemetry.addLine("Press B to stop the test.")
                telemetry.update()
            }
        } finally {
            arm.power = 0.0
        }
    }

    private class ArmPid(private val kP: Double, private val kI: Double, private val kD: Double) {
        companion object { private const val MAX_INTEGRAL = 300.0 } // degree-seconds

        private var integral = 0.0
        private var previousAngle = 0.0
        private var previousTimeNanos = 0L

        fun calculate(targetAngle: Double, currentAngle: Double): Double {
            val now = System.nanoTime()
            val error = targetAngle - currentAngle

            if (previousTimeNanos == 0L) {
                previousTimeNanos = now
                previousAngle = currentAngle
                return kP * error
            }

            val dt = (now - previousTimeNanos) / 1_000_000_000.0
            previousTimeNanos = now
            if (dt <= 0.0 || dt > 0.1) {
                previousAngle = currentAngle
                return kP * error + kI * integral
            }

            integral += error * dt
            integral = max(-MAX_INTEGRAL, min(MAX_INTEGRAL, integral))
            val degreesPerSecond = (currentAngle - previousAngle) / dt
            previousAngle = currentAngle

            return kP * error + kI * integral - kD * degreesPerSecond
        }

        fun reset() {
            integral = 0.0
            previousAngle = 0.0
            previousTimeNanos = 0L
        }
    }
}

Tuning telemetry

1. Select Guided PID Arm, initialize, and confirm the displayed angle matches the real arm's zero position.
2. Press D-pad left for the 30° target. Watch the target, measured angle, error, and power together.
3. If the measured angle moves the wrong way, press B to stop and fix encoder or motor direction before continuing.
4. If it moves correctly but stops short or overshoots, adjust only KP first. Record each value and result.
5. Once P moves smoothly, add a little KD if it passes the target. Add KI only if repeated tests show a small persistent offset.
6. Test the full safe range and expected load before relying on the arm in autonomous or a match.

A build only checks that the code compiles. It does not prove the encoder scale, motor direction, gains, or mechanism limits are safe.

Troubleshooting

Manual override

Manual input and a position controller need one owner for the motor command. Select the mode, calculate its request, apply limits, then write power once per loop.

Opposite jog inputs must produce opposite directions. Define what happens when both are pressed. On a mode change, clear or reinitialise integral and derivative state so old error does not produce a sudden command.

A motor retains its previous power request until changed. Omitting the manual-release branch can leave it running. Zero output may let a vertical load fall; use the mechanism's established hold/support policy and test the transition under representative load.

Next steps

Once the arm's encoder and feedback loop make sense, continue to motion profiling to control how quickly a mechanism accelerates, or review low-pass filters if sensor readings are noisy.

For a continuously spinning shooter, use Flywheel velocity control to choose between the hub's velocity loop and your own controller, convert encoder units, and measure recovery before feeding.

Check SDK-specific motor and OpMode behavior separately from the PID equations. Tune gains using your mechanism and measurements; the example values are not calibrated for your robot.