Skip to content

2D Plate Balancing (Servos + Camera)

Time estimate: ~120 minutes Prerequisites: 1D Ball Balancing, Ball Position Detection

Learning Objectives

By the end of this tutorial you will be able to:

  • Build a 2-axis gimbal using two servos to tilt a plate in X and Y
  • Integrate the ball detection pipeline from the previous tutorial into a control loop
  • Implement a 2-axis PID controller for plate balancing
  • Measure the full sensor-to-actuator pipeline latency
  • Identify which pipeline stage limits control bandwidth
Pipeline Latency in Vision-Based Control

In a vision-based control loop, every pipeline stage adds latency: camera exposure, image readout (DMA), image processing (threshold, morphology, contour), PID computation, and servo mechanical response. The total sensor-to-actuator delay directly limits achievable control bandwidth — a loop with 100 ms total latency cannot correct disturbances faster than ~10 Hz.

The Pi Camera Module connects via CSI, transferring frames through DMA with near-zero transport delay. This makes it well suited for real-time control — the bottleneck shifts to image processing and servo response rather than camera transport.

This tutorial applies concepts from both real-time systems (2-axis PID, Ziegler-Nichols tuning) and the camera/vision pipeline built in previous tutorials.

See also: Real-Time Systems reference | Real-Time Graphics reference

Introduction

Plate balancing extends the 1D beam problem to two dimensions. A ball rolls freely on a flat plate, and two servos tilt the plate in X and Y. The sensor is no longer a simple distance measurement — instead, a camera looks down at the plate and OpenCV detects the ball's position in real time using the contrast-based pipeline from the Ball Position Detection tutorial.

This creates a significantly more challenging real-time pipeline:

Camera captures frame (CSI / DMA)
    → Image processing (threshold + morphology + circularity)
    → 2-axis PID calculates X and Y corrections
    → Two servos tilt the plate
    → Ball position changes
    → Next camera frame captures new position

Every stage adds latency. The camera capture alone takes 16-33 ms (30-60 fps), and image processing adds more. This tutorial measures each stage and shows how latency accumulates in a real control system.

Hardware

  • Raspberry Pi 4 with Pi Camera Module (CSI)
  • 2× MG996R servos — X-axis and Y-axis tilt
  • Flat plate — 20×20 cm rigid surface (acrylic, wood, or 3D-printed)
  • Gimbal mount — two servos arranged at 90° to tilt the plate in both axes
  • Ball — high contrast against the plate (white ball on dark plate or vice versa)
Warning

Two MG996R servos under load can draw 5A combined. Use a separate 5-6V power supply rated for at least 5A. Never power servos from the Pi GPIO header.

1. Mechanical Assembly

Concept: The two servos must be mounted orthogonally so that one tilts the plate in X and the other in Y, with the plate balanced at the intersection.

Gimbal Layout

            Camera (overhead, looking down)
                │
                ▼
        ┌───────────────┐
        │               │
        │     plate     │
        │    ● ball     │
        │               │
        └───────┬───────┘
                │
          ┌─────┴─────┐
          │  Y-servo  │  ← tilts plate front/back
          └─────┬─────┘
                │
          ┌─────┴─────┐
          │  X-servo  │  ← tilts plate left/right
          └───────────┘
            (base)

The bottom servo rotates the top servo and the plate together (X-axis tilt). The top servo tilts the plate directly (Y-axis tilt). Mount the camera above the plate, looking straight down.

Tip

For a quick prototype, use a universal joint or ball joint at the plate center, with two servo arms connected by stiff wire (push-pull linkage) to the plate edges. This avoids building a full gimbal frame.

2. Two-Axis Servo Control

Concept: Each servo controls one tilt axis. The control is identical to the 1D tutorial — sysfs PWM — but you need two PWM channels.

Enable Two PWM Channels

Add to /boot/firmware/config.txt:

dtoverlay=pwm-2chan,pin=18,func=2,pin2=19,func2=2
Two-Channel PWM: Hardware and Device Tree

The pwm-2chan overlay extends the single-channel overlay from the 1D tutorial to use both PWM hardware channels:

  • pin=18,func=2 → GPIO18 = PWM0 channel 0 (ALT5)
  • pin2=19,func2=2 → GPIO19 = PWM0 channel 1 (ALT5)

The BCM2711 has two independent PWM channels sharing a single clock source. Both channels run from the same base frequency (19.2 MHz) but have independent period and duty cycle registers, so they can output different waveforms simultaneously. This is important: changing one servo's angle does not affect the other's timing.

Verify after reboot: 

cat /sys/class/pwm/pwmchip0/npwm  # Should show: 2

For a custom image, ensure CONFIG_PWM_BCM2835=y and include both PWM channels in the device tree pinctrl node with brcm,pins = <18 19>; brcm,function = <2>;.

Reboot and export both channels:

echo 0 | sudo tee /sys/class/pwm/pwmchip0/export  # GPIO18 → X servo
echo 1 | sudo tee /sys/class/pwm/pwmchip0/export  # GPIO19 → Y servo

# Configure both for 50 Hz servo PWM
for ch in pwm0 pwm1; do
    echo 20000000 | sudo tee /sys/class/pwm/pwmchip0/$ch/period
    echo 1500000  | sudo tee /sys/class/pwm/pwmchip0/$ch/duty_cycle
    echo 1        | sudo tee /sys/class/pwm/pwmchip0/$ch/enable
done

Test Both Axes

python3 - <<'PY'
import time

def set_servo(channel, degrees):
    degrees = max(60, min(120, degrees))
    pulse_ns = int((1000 + degrees / 180.0 * 1000) * 1000)
    with open(f"/sys/class/pwm/pwmchip0/pwm{channel}/duty_cycle", "w") as f:
        f.write(str(pulse_ns))

# Sweep X axis
for a in range(80, 101, 2):
    set_servo(0, a); time.sleep(0.1)
set_servo(0, 90)

# Sweep Y axis
for a in range(80, 101, 2):
    set_servo(1, a); time.sleep(0.1)
set_servo(1, 90)

print("Both servos centered")
PY
Checkpoint

Both servos move independently. The plate tilts in X when servo 0 moves and in Y when servo 1 moves.

3. Ball Detection (Recap)

Concept: This step uses the contrast-based detection pipeline from the Ball Position Detection tutorial. If you haven't completed that tutorial, do it first — the detection approach (thresholding + morphological opening + circularity filter) is explained step by step there.

Quick verification that detection works with the plate and camera in their final positions:

python3 - <<'PY'
import cv2, numpy as np, time, math
from picamera2 import Picamera2

picam2 = Picamera2()
config = picam2.create_preview_configuration(main={"size": (320, 240), "format": "RGB888"})
picam2.configure(config)
picam2.start()
time.sleep(1)

INVERT = True   # True = dark ball on light surface, False = light ball on dark
MORPH_SIZE = 11
MIN_CIRCULARITY = 0.6
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (MORPH_SIZE, MORPH_SIZE))

for i in range(20):
    frame = picam2.capture_array()
    gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
    blurred = cv2.GaussianBlur(gray, (9, 9), 0)
    thresh_type = cv2.THRESH_BINARY_INV if INVERT else cv2.THRESH_BINARY
    _, binary = cv2.threshold(blurred, 0, 255, thresh_type + cv2.THRESH_OTSU)
    binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel)
    contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    for c in sorted(contours, key=cv2.contourArea, reverse=True):
        area = cv2.contourArea(c)
        if area < 300:
            continue
        p = cv2.arcLength(c, True)
        if p == 0:
            continue
        circ = 4 * math.pi * area / (p * p)
        if circ > MIN_CIRCULARITY:
            M = cv2.moments(c)
            if M["m00"] > 0:
                cx = int(M["m10"] / M["m00"])
                cy = int(M["m01"] / M["m00"])
                print(f"Ball at ({cx:3d}, {cy:3d})  area={area:.0f}  circ={circ:.2f}")
            break
    else:
        print("No ball detected")

picam2.stop()
PY
Checkpoint

The ball is detected consistently with correct coordinates. If not, adjust INVERT or MORPH_SIZE — refer to the Ball Position Detection troubleshooting tips.

4. Two-Axis PID Controller

Concept: The 2D controller runs two independent PID loops — one for X, one for Y. The camera provides both coordinates in each frame.

Tip

The PID tuning methodology and Ziegler-Nichols procedure are derived in Real-Time Systems reference § Control Theory Foundations. In 2D, each axis is tuned independently.

mkdir -p ~/ball-balance
cat > ~/ball-balance/balance_2d.py << 'EOF'
#!/usr/bin/env python3
"""2D plate balancing: 2-axis PID with camera ball detection."""
import cv2, time, csv, math
from picamera2 import Picamera2

# ── Camera setup ────────────────────────────────────
picam2 = Picamera2()
config = picam2.create_preview_configuration(main={"size": (320, 240), "format": "RGB888"})
picam2.configure(config)
picam2.start()
time.sleep(1)  # let auto-exposure settle

# ── Ball detection parameters ──────────────────────
INVERT = True           # True = dark ball on light surface
MORPH_SIZE = 11
MIN_AREA = 300
MIN_CIRCULARITY = 0.6
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (MORPH_SIZE, MORPH_SIZE))

# ── Servo setup ─────────────────────────────────────
def set_servo(channel, degrees):
    degrees = max(60, min(120, degrees))
    pulse_ns = int((1000 + degrees / 180.0 * 1000) * 1000)
    with open(f"/sys/class/pwm/pwmchip0/pwm{channel}/duty_cycle", "w") as f:
        f.write(str(pulse_ns))

# ── PID class ───────────────────────────────────────
class PID:
    def __init__(self, kp, ki, kd, limit=30):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.limit = limit
        self.integral = 0.0
        self.prev_error = 0.0

    def update(self, error, dt):
        self.integral += error * dt
        self.integral = max(-200, min(200, self.integral))
        derivative = (error - self.prev_error) / max(dt, 0.001)
        self.prev_error = error
        out = self.kp * error + self.ki * self.integral + self.kd * derivative
        return max(-self.limit, min(self.limit, out))

# ── Controller ──────────────────────────────────────
# Image center = plate center (adjust if camera is offset)
SETPOINT_X = 160  # pixels (half of 320)
SETPOINT_Y = 120  # pixels (half of 240)

pid_x = PID(kp=0.06, ki=0.001, kd=0.04)
pid_y = PID(kp=0.06, ki=0.001, kd=0.04)
BASE_ANGLE = 90.0

# ── Logging ─────────────────────────────────────────
LOG_FILE = "/tmp/balance_2d_log.csv"
log = open(LOG_FILE, "w", newline="")
writer = csv.writer(log)
writer.writerow(["time_ms", "ball_x", "ball_y", "err_x", "err_y",
                 "out_x", "out_y", "angle_x", "angle_y",
                 "capture_ms", "process_ms", "total_ms"])

def detect_ball(frame_rgb):
    """Detect ball using contrast threshold + morphology + circularity."""
    gray = cv2.cvtColor(frame_rgb, cv2.COLOR_RGB2GRAY)
    blurred = cv2.GaussianBlur(gray, (9, 9), 0)
    thresh_type = cv2.THRESH_BINARY_INV if INVERT else cv2.THRESH_BINARY
    _, binary = cv2.threshold(blurred, 0, 255, thresh_type + cv2.THRESH_OTSU)
    binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel)
    contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    for c in sorted(contours, key=cv2.contourArea, reverse=True):
        area = cv2.contourArea(c)
        if area < MIN_AREA:
            break
        p = cv2.arcLength(c, True)
        if p == 0:
            continue
        circ = 4 * math.pi * area / (p * p)
        if circ > MIN_CIRCULARITY:
            M = cv2.moments(c)
            if M["m00"] > 0:
                return int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])
    return None, None

# ── Control loop ────────────────────────────────────
print(f"2D Plate Balance — setpoint=({SETPOINT_X}, {SETPOINT_Y})")
print("Press Ctrl+C to stop.")

t_start = time.monotonic()
t_prev = t_start
iteration = 0

try:
    while True:
        t0 = time.monotonic()

        # Capture frame
        frame = picam2.capture_array()
        t_capture = time.monotonic()

        # Detect ball
        ball_x, ball_y = detect_ball(frame)
        t_process = time.monotonic()

        if ball_x is None:
            ball_x, ball_y = SETPOINT_X, SETPOINT_Y  # hold position if lost

        # PID
        dt = t0 - t_prev
        t_prev = t0
        if dt < 0.001: dt = 0.001

        err_x = SETPOINT_X - ball_x
        err_y = SETPOINT_Y - ball_y
        out_x = pid_x.update(err_x, dt)
        out_y = pid_y.update(err_y, dt)

        angle_x = BASE_ANGLE + out_x
        angle_y = BASE_ANGLE + out_y
        set_servo(0, angle_x)
        set_servo(1, angle_y)

        # Timing
        t_end = time.monotonic()
        capture_ms = (t_capture - t0) * 1000
        process_ms = (t_process - t_capture) * 1000
        total_ms = (t_end - t0) * 1000

        elapsed_ms = (t0 - t_start) * 1000
        writer.writerow([f"{elapsed_ms:.1f}", ball_x, ball_y,
                        f"{err_x:.1f}", f"{err_y:.1f}",
                        f"{out_x:.2f}", f"{out_y:.2f}",
                        f"{angle_x:.1f}", f"{angle_y:.1f}",
                        f"{capture_ms:.1f}", f"{process_ms:.1f}",
                        f"{total_ms:.1f}"])

        iteration += 1
        if iteration % 30 == 0:
            print(f"  ball=({ball_x:3d},{ball_y:3d})  "
                  f"err=({err_x:+5.0f},{err_y:+5.0f})  "
                  f"cap={capture_ms:4.1f}ms proc={process_ms:4.1f}ms "
                  f"total={total_ms:5.1f}ms")

except KeyboardInterrupt:
    set_servo(0, 90)
    set_servo(1, 90)
    picam2.stop()
    log.close()
    print(f"\nStopped. Log saved to {LOG_FILE}")
    print(f"Total iterations: {iteration}")
EOF

Run the controller:

python3 ~/ball-balance/balance_2d.py
Checkpoint

The script runs, detects the ball, and adjusts both servos. The terminal shows ball position, errors, and timing per iteration.

5. Pipeline Latency Budget

Concept: The total latency from ball movement to servo correction is the sum of all pipeline stages. Measuring each stage tells you where the bottleneck is and whether the loop is fast enough.

Ball moves on plate
    → Camera exposure (fixed by shutter/FPS)         ~16 ms (60 fps)
    → Image readout via CSI (DMA)                     ~1 ms
    → Threshold + morphology + contour finding         ~3-8 ms
    → PID calculation                                  ~0.1 ms
    → PWM update via sysfs                             ~0.5 ms
    → Servo mechanical response                        ~20-50 ms
    ─────────────────────────────────────────────────
    Total:  ~40-75 ms  →  ~13-25 Hz control rate

The servo's mechanical response time (how fast it physically moves to the new angle) is often the largest contributor. A digital servo with faster response would improve control quality.

Fill In Your Measured Pipeline

Stage Measured (ms)
Camera capture _
Image processing (threshold + morphology + contour) _
PID + servo write _
Total loop time _
Effective control rate (Hz) _
Tip

The CSV log at /tmp/balance_2d_log.csv contains per-iteration timing. Import it into a spreadsheet or use Python to compute min/max/average for each stage.

6. PID Tuning

Concept: Start with one axis at a time. Lock the ball's movement to one direction (e.g., place guide rails) and tune that axis before enabling both.

Tuning Procedure

  1. Set Ki=0, Kd=0. Increase Kp until the ball oscillates around the setpoint. Note this value as K_u (ultimate gain).
  2. Add Kd. Increase Kd to dampen the oscillation. The ball should settle without overshooting.
  3. Add Ki. A small Ki eliminates steady-state error (ball resting slightly off-center). Too much Ki causes slow oscillation.
  4. Repeat for the other axis.

Validation

Run the controller and gently push the ball off-center. Measure:

Metric Value
Settling time (seconds to return within ±10px) _
Overshoot (max pixels past setpoint) _
Steady-state error (average offset at rest) _
Checkpoint

The ball returns to center after a disturbance and stays within ±10 pixels of the setpoint.

What Just Happened?

You built a 2-axis real-time control system with computer vision feedback:

Pi Camera (CSI/DMA)
    → Threshold + morphology + circularity → Ball (x, y)
    → PID X controller → Servo X (GPIO18)
    → PID Y controller → Servo Y (GPIO19)
    → Plate tilts → Ball moves → Camera sees new position

The key insight is that every component in the loop adds latency, and that latency directly affects control quality. If the total loop time exceeds ~80 ms, the controller is acting on stale position data and the ball becomes harder to balance.

This is the core principle of real-time embedded systems: it's not about how fast you can compute, it's about how consistently and quickly the entire sensor-to-actuator loop runs.

Challenges

Challenge 1: Path Following

Instead of balancing at a fixed point, define a circular or figure-eight path and make the ball follow it. Update the setpoint coordinates each frame to trace the path. How fast can the ball track the moving target?

Challenge 2: Standard vs PREEMPT_RT Under Load

Run the 2D controller on both standard and PREEMPT_RT kernels while stress-ng --cpu 4 generates CPU load. Compare the maximum loop time and ball stability. Does the RT kernel prevent the worst-case timing spikes?

Metric Standard Standard + Load PREEMPT_RT RT + Load
Avg loop time (ms)
Max loop time (ms)
Ball stable?
Challenge 3: Display Overlay

Add a pygame display that shows the camera feed with ball position overlay, PID output bars, and timing info — while the control loop continues running. Use a separate thread for rendering so the display does not slow down the control loop.

Challenge 4: Offload to Pico 2

If the Linux control loop is too slow or jittery under load, offload the PID + servo control to a Raspberry Pi Pico 2 connected via UART. The Pi 4 handles camera capture and ball detection, sends (x, y) coordinates over serial, and the Pico 2 runs the PID loop at a fixed rate with hardware PWM. Compare the control quality with the all-Linux approach. See MCU Real-Time Controller for the architecture.

Deliverable

  • [ ] Ball balances on the 2D plate (at least briefly)
  • [ ] Pipeline latency budget measured (per-stage timing)
  • [ ] PID tuning table filled in (settling time, overshoot, steady-state error)
  • [ ] Brief note: which pipeline stage is the bottleneck? What would you change to make it faster?

Course Overview | Previous: ← Ball Position Detection | 1D Ball Balancing