01 | PID IS THREE WAYS TO RESPOND TO ERROR

You already know the proportional rule: output grows with the remaining error. PID adds two more ways to shape the correction.

Start with P only. A well-tuned P or PD controller may be enough for a drivetrain move. Adding integral automatically can make a previously reliable routine worse.

02 | INCLUDE TIME IN THE CALCULATION

For a constant target, a discrete controller can use:

# dt is elapsed time in seconds.
integral = integral + error * dt
error_rate = (error - previous_error) / dt
output = kp * error + ki * integral + kd * error_rate

These equations describe the basic idea, not a complete motor-control loop. Real code also needs initialization, output limits, feedback checks and a finish condition.

If error stays at 20 mm for 0.5 seconds, it adds 10 mm·s to the integral. If error falls from 100 mm to 80 mm in 0.02 seconds, its rate is -1000 mm/s.

With positive forward error, that negative rate gives a negative D contribution. It reduces the positive request during a fast approach. D does not predict a collision or guarantee a stop.

03 | GAINS HAVE UNITS

Changing millimetres to centimetres or seconds to milliseconds changes the numerical gains needed. Copying values from a different implementation is not a tuning method.

This guide commands VEX motor velocity percentage. The V5 motor has its own internal control behaviour. These outer position-loop gains are not directly interchangeable with voltage-control gains.

04 | DEFINE WHAT FINISHED MEANS

Being close to the target for one sample is not enough. A robot can pass through a ±10 mm window at speed and leave it on the next update.

The starter requires three conditions:

1. Both side errors are within the position tolerance.
2. Both estimated error rates are within the rate tolerance.
3. Both conditions remain true continuously for the settling interval.

For a fixed target, error rate is the negative of measured position rate. Its magnitude can therefore serve as an estimated speed check. It is affected by encoder resolution and noise.

The example stops output within the position window, then observes whether movement slows and remains there. It is a move-and-stop controller, not an active precision-hold loop.

05 | TOLERANCE, SETTLING INTERVAL AND TIMEOUT

Measured settling time is the time from the start of a move until it meets the finish policy. The required settling interval is just the final continuous observation period.

A timeout is not a successful finish. If you loosen tolerances merely to avoid timeouts, you may hide a controller or mechanical problem.

06 | READ A SETTLING TRACE

Check every control update, not just the rows in a summary table. A single failed sample resets the continuous settling timer.

The first derivative sample has no earlier measurement to compare with. The starter does not treat that first sample as proof that the robot is stationary.

07 | TUNE P, THEN D, THEN CONSIDER I

1. Prepare: verify motor signs, encoder conversion, inertial calibration and stopping mode.
2. Fix the test: use the same target, starting mark, output cap and loop policy.
3. Set Ki and Kd to zero: begin with a small positive Kp.
4. Increase Kp gradually: compare response time, overshoot and final physical error.
5. Add a small Kd if needed: test whether it reduces oscillation without excessive noise or slow response.
6. Consider Ki last: use it only for a repeatable persistent offset after other causes have been investigated.
7. Validate: repeat forward, reverse and turns in both directions with normal load.

Keep the output cap conservative during initial tests. Deliberately creating violent oscillation to find a critical gain is unnecessary for this course.

Change one gain at a time. Distance gains and turn gains should have separate records.

08 | A BOUNDED PID STARTER

This is a complete autonomous-only teaching project using the same four-motor configuration as 05.1. It begins with Ki = 0 and Kd = 0, so your first comparison is against P control with stricter finish checks.

Keep generated VEX imports and the Brain above it, and paste below # Begin project code. Do not duplicate its manually declared devices in Devices. Check the listed ports, wheel geometry and reversal settings on your robot before enabling movement.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | PID TUNING STARTER
# =============================================================================
# Keep generated imports and brain above '# Begin project code'.
# Manual configuration below: do not duplicate these devices in Devices.
# Left motors: ports 3 and 4; right motors: ports 1 and 2.
# Blue cartridges. Reversal settings must match your physical robot.
# Inertial: Smart Port 20. Controller B cancels movement.
import math

left_motor_1 = Motor(Ports.PORT3, GearSetting.RATIO_6_1, False)
left_motor_2 = Motor(Ports.PORT4, GearSetting.RATIO_6_1, False)
right_motor_1 = Motor(Ports.PORT1, GearSetting.RATIO_6_1, True)
right_motor_2 = Motor(Ports.PORT2, GearSetting.RATIO_6_1, True)
left_drive = MotorGroup(left_motor_1, left_motor_2)
right_drive = MotorGroup(right_motor_1, right_motor_2)
inertial = Inertial(Ports.PORT20)
controller = Controller(PRIMARY)

# =============================================================================
# MEASURED GEOMETRY AND STARTER GAINS (EXAMPLES, NOT TUNED VALUES)
# =============================================================================
MOTION_CONFIGURED = False       # Enable after direction/geometry checks
WHEEL_DIAMETER_MM = 100.0       # Replace with effective measured diameter
WHEEL_TURNS_PER_MOTOR_TURN = 1.0 # Direct drive = 1; external gearing changes it
DRIVE_GAINS = (0.08, 0.0, 0.0) # Kp, Ki, Kd: begin with P only
TURN_GAINS = (0.7, 0.0, 0.0)
DRIVE_RATE_TOL = 30.0           # mm/s, from distance error change
TURN_RATE_TOL = 8.0             # degrees/s
DRIVE_I_ZONE = 100.0            # mm; relevant only when Ki > 0
TURN_I_ZONE = 10.0              # degrees
I_OUTPUT_CAP = 2.0             # Limit integral contribution to 2%
DERIVATIVE_FILTER_S = 0.05      # First-order D filter time constant
MAX_LOOP_GAP_S = 0.10           # Reject a delayed control iteration
MAX_OUTPUT = 25.0               # Conservative test cap
DRIVE_TOL_MM = 10.0
TURN_TOL_DEG = 2.0
SETTLE_MS = 200
LOOP_MS = 20
ready = False

# =============================================================================
# SHARED OUTPUT AND FEEDBACK
# =============================================================================
def clamp(value, low, high):
    return max(low, min(high, value))

def stop_drive():
    left_drive.stop(BRAKE)
    right_drive.stop(BRAKE)

def side_output(group, value):
    if value == 0:
        group.stop(BRAKE)
    else:
        group.spin(FORWARD if value > 0 else REVERSE, abs(value), PERCENT)

def heading_error(target, current):
    return (target - current + 180) % 360 - 180

def side_mm(motor_a, motor_b):
    turns = (motor_a.position(TURNS) + motor_b.position(TURNS)) / 2
    return turns * math.pi * WHEEL_DIAMETER_MM * WHEEL_TURNS_PER_MOTOR_TURN

def permitted():
    return (ready and MOTION_CONFIGURED and competition.is_enabled()
            and competition.is_autonomous()
            and not controller.buttonB.pressing()
            and inertial.installed() and not inertial.is_calibrating())

# =============================================================================
# PID STATE: ONE INSTANCE PER CONTROLLED SIDE, RESET FOR EACH MOVE
# =============================================================================
class PID:
    def __init__(self, kp, ki, kd, i_zone, wrapped=False):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.i_zone = i_zone
        self.wrapped = wrapped
        self.integral = 0.0
        self.previous = None
        self.filtered_rate = 0.0

    def update(self, error, dt):
        # Caller rejects non-finite error and unsuitable dt first.
        first = self.previous is None
        delta = 0.0 if first else error - self.previous
        if self.wrapped and not first:
            delta = (delta + 180) % 360 - 180
        rate = None if first else delta / dt
        raw_rate = 0.0 if first else rate
        alpha = dt / (DERIVATIVE_FILTER_S + dt)
        self.filtered_rate += alpha * (raw_rate - self.filtered_rate)
        p = self.kp * error
        d = self.kd * self.filtered_rate

        # Clear stored error outside the I zone or on an error sign change.
        if (self.ki <= 0 or abs(error) > self.i_zone
                or (not first and error * self.previous < 0)):
            self.integral = 0.0
        else:
            candidate = self.integral + error * dt
            candidate = clamp(candidate, -I_OUTPUT_CAP / self.ki,
                              I_OUTPUT_CAP / self.ki)
            trial = p + self.ki * candidate + d
            # Freeze accumulation that would push further into saturation.
            if abs(trial) <= MAX_OUTPUT or trial * error < 0:
                self.integral = candidate
        i = self.ki * self.integral
        self.previous = error
        return clamp(p + i + d, -MAX_OUTPUT, MAX_OUTPUT), p, i, d, rate


def run_pid(errors_fn, gains, tolerance, rate_tolerance,
            i_zone, timeout_ms, wrapped=False):
    stop_drive()
    if (gains[0] <= 0 or gains[1] < 0 or gains[2] < 0
            or tolerance <= 0 or rate_tolerance <= 0 or timeout_ms <= 0):
        return "invalid configuration"
    left_pid = PID(*gains, i_zone, wrapped)
    right_pid = PID(*gains, i_zone, wrapped)
    started = brain.timer.time(MSEC)
    previous_time = started
    settled_since = None
    try:
        while True:
            if not permitted():
                return "cancelled or not ready"
            now = brain.timer.time(MSEC)
            if now - started >= timeout_ms:
                return "timeout"
            dt = (now - previous_time) / 1000.0
            if dt <= 0:
                wait(LOOP_MS, MSEC)
                continue
            if dt > MAX_LOOP_GAP_S:
                return "loop timing fault"
            previous_time = now
            errors = errors_fn()
            if not all(math.isfinite(v) for v in errors):
                return "invalid feedback"
            left_data = left_pid.update(errors[0], dt)
            right_data = right_pid.update(errors[1], dt)
            near = all(abs(v) <= tolerance for v in errors)
            slow = (left_data[4] is not None and right_data[4] is not None
                    and abs(left_data[4]) <= rate_tolerance
                    and abs(right_data[4]) <= rate_tolerance)
            if near:
                # Brake inside the position window, even if still moving.
                stop_drive()
                left_pid.integral = right_pid.integral = 0.0
            else:
                side_output(left_drive, left_data[0])
                side_output(right_drive, right_data[0])
            if near and slow:
                if settled_since is None:
                    settled_since = now
                if now - settled_since >= SETTLE_MS:
                    return "settled"
            else:
                settled_since = None
            wait(LOOP_MS, MSEC)
    except Exception:
        return "feedback or command error"
    finally:
        stop_drive()

# =============================================================================
# REUSABLE MOVEMENT FUNCTIONS
# =============================================================================
def drive_distance_pid(distance_mm, timeout_ms=4000):
    left_start = side_mm(left_motor_1, left_motor_2)
    right_start = side_mm(right_motor_1, right_motor_2)
    def errors():
        left_delta = side_mm(left_motor_1, left_motor_2) - left_start
        right_delta = side_mm(right_motor_1, right_motor_2) - right_start
        return distance_mm - left_delta, distance_mm - right_delta
    return run_pid(errors, DRIVE_GAINS, DRIVE_TOL_MM, DRIVE_RATE_TOL,
                   DRIVE_I_ZONE, timeout_ms)

def turn_to_heading_pid(target_deg, timeout_ms=3000):
    target = target_deg % 360
    def errors():
        error = heading_error(target, inertial.heading(DEGREES))
        # Positive heading error: turn clockwise.
        return error, -error
    return run_pid(errors, TURN_GAINS, TURN_TOL_DEG, TURN_RATE_TOL,
                   TURN_I_ZONE, timeout_ms, wrapped=True)

def turn_by_pid(angle_deg, timeout_ms=3000):
    # Shortest-path relative turn; restrict requested angle to under 180.
    if abs(angle_deg) >= 180:
        stop_drive()
        return "relative angle must be less than 180"
    return turn_to_heading_pid(inertial.heading(DEGREES) + angle_deg, timeout_ms)

# =============================================================================
# COMPETITION ENTRY POINTS: ONE OWNER OF DRIVE OUTPUT
# =============================================================================
def autonomous():
    stop_drive()
    if not permitted():
        return
    for name, action in (
        ("forward", lambda: drive_distance_pid(500)),
        ("right turn", lambda: turn_by_pid(90)),
        ("reverse", lambda: drive_distance_pid(-250))
    ):
        result = action()
        print(name, result)
        if result != "settled":
            break  # Do not continue the route after a failed move.
        wait(300, MSEC)
    stop_drive()

def driver_control():
    # Teaching project: intentionally no driver movement.
    stop_drive()

competition = Competition(driver_control, autonomous)
stop_drive()
# Calibrate while disabled and stationary, before autonomous is enabled.
# calibrate() may block; movement timeouts do not cover this setup call.
if MOTION_CONFIGURED and not competition.is_enabled() and inertial.installed():
    inertial.calibrate()
    ready = not inertial.is_calibrating() and not competition.is_enabled()
print("PID controller ready:", ready)
while True:
    wait(100, MSEC)

Each move creates fresh PID state. The code measures actual elapsed time and returns a timing fault after a gap longer than 100 ms. It does not pretend a long delayed iteration was a normal 20 ms update.

The derivative uses a simple first-order filter. The finish speed check uses the unfiltered error rate, so noisy feedback can still prevent settling. Choose rate tolerances from recorded measurement behaviour.

The integral has a small contribution cap, an activation zone and a saturation check. These are teaching protections, not a universally optimal anti-windup method. Section 05.3 examines their tradeoffs.

Calibration may block before the route begins. Movement timeouts do not supervise it. Ready remains false if setup starts enabled. Integrate the functions into your existing competition structure without creating a second output owner.

09 | UNDERSTAND THE EXAMPLE LIMITATIONS

The distance controller checks each side separately. It estimates travel from motor encoders and does not correct wheel slip or verify field arrival.

The turn controller uses shortest-path wrapped heading. Its derivative also wraps the change between errors to avoid a false spike at the heading boundary. Relative requests of 180 degrees or more remain rejected.

Rate calculations assume a fixed target for each move. Changing the target mid-loop can create a derivative kick; tracking a changing path needs a deliberate design.

Controller B, mode and readiness guards abort ordinary movement. Exceptions stop through finally. Forced task termination can bypass ordinary cleanup; test competition transitions and preserve one drivetrain output owner.

10 | RECORD EVIDENCE, NOT JUST A FAVOURITE GAIN

Measure overshoot as movement beyond the target in the commanded direction. For turns, handle the heading wrap when comparing positions.

Log telemetry at a lower rate than the control updates. Heavy printing can delay the loop and distort the experiment. Save the gains with the hardware configuration and test date.

A useful result combines low physical error, acceptable completion time and repeatability. Compare at least three trials before drawing a conclusion.

11 | DIAGNOSE BEFORE ADDING ANOTHER TERM

CHALLENGES | TUNE AND JUSTIFY

1. Calculate: find integral accumulation for 15 mm error held for 0.4 seconds.
2. Calculate: find error rate when error drops from 60 mm to 40 mm in 20 ms.
3. Trace: explain why the settling timer resets in the example table.
4. Compare: record three P-only trials, then three trials with a small D gain.
5. Choose: justify position and rate tolerances for one real scoring or alignment task.
6. Test: simulate a long loop delay and confirm failure rather than success.
7. Evaluate: decide whether I is needed using evidence of repeatable persistent error.
8. Document: save your selected gains and explain the tradeoff between speed, accuracy and repeatability.

OFFICIAL REFERENCES

• VEX motor feedback and output API
• VEX heading and calibration API
• WPILib PID concepts: timing, tolerances and state

WPILib is a conceptual reference, not a VEX library to import. The VEX Python controller and challenges here are independently written examples. VEX and VEXcode are trademarks of their respective owners.