Motion Control and PID — Robotics

TL;DR

A motion is planned as a trajectory (where to be at every moment), usually with a trapezoidal velocity profile: accelerate, cruise, decelerate.
A PID controller turns the error between plan and measurement into a motor command; on robots it runs at a fixed, fast rate.
P alone leaves a steady error against a constant disturbance; the I term removes it, and anti-windup stops the I term from causing a large overshoot while the output is at its limit.
Servo drives nest loops inside each other (position, velocity, current), each faster than the loop around it.
Feedforward adds the command the plan already predicts, so the feedback loop only corrects what is left and the following error shrinks.

From a plan to motion

Kinematics says which joint angles put the hand at a point. It does not say how to get there: how fast, how hard to accelerate, and what to do when the motor lags behind. That is motion control, and it starts with a trajectory: a path with timing, the position (and so the speed and acceleration) the robot should have at every moment. A path says "along this line"; a trajectory says "here at 0.5 seconds, there at 1.0".

A typical motion system has layers:

A planner chooses the path (see Path Planning).
A trajectory generator adds timing within the limits of the machine: maximum speed, acceleration and sometimes jerk.
A controller per joint or wheel compares the trajectory with what the encoders measure and computes a command.
A drive turns the command into current through the motor (see Motors and Drives).

Steps 3 and 4 form a closed control loop: measure, compare with the setpoint, correct, repeat. The OT topic explains the loop and the PID controller with a heated water tank; this page applies them to motors and moving robots.

Trapezoidal velocity profiles

A motor cannot jump from standstill to full speed: that would need infinite acceleration and so infinite torque. The simplest realistic plan is a trapezoidal velocity profile: accelerate at a constant rate a up to a maximum speed v_{\max}, cruise, then decelerate at the same rate to stop exactly at the target. Drawn as speed against time, it is a trapezoid, and the area under it is the distance.

A 1.0 m move at up to 0.5 m/s with 1.0 m/s² acceleration: 0.5 s speeding up, 1.5 s cruising, 0.5 s braking. A 0.1 m move (dashed) never reaches full speed and becomes a triangle.

The time to reach full speed and the distance covered meanwhile are

t_a = \frac{v_{\max}}{a}, \qquad d_a = \tfrac{1}{2} a t_a^2, \qquad t_c = \frac{d - 2 d_a}{v_{\max}}, \qquad T = 2 t_a + t_c

Worked example: 1.0 m with v_{\max} = 0.5 m/s and a = 1.0 m/s².

Accelerating takes 0.5 / 1.0 = 0.5 s and covers ½ × 1.0 × 0.5² = 0.125 m.
Braking takes the same: another 0.5 s and 0.125 m.
That leaves 1.0 − 0.25 = 0.75 m at 0.5 m/s: 1.5 s of cruising.
Total: 0.5 + 1.5 + 0.5 = 2.5 s.

If the move is so short that 2 d_a \ge d, the robot never reaches v_{\max}: it accelerates for half the distance and brakes for the other half, and the trapezoid becomes a triangle with

t_a = \sqrt{d / a}, \qquad v_{\text{peak}} = a \, t_a = \sqrt{a d}

import math

def trapezoid(distance, v_max, accel):
    """Times (s) and peak speed (m/s) of a move that starts and ends at rest."""
    t_acc = v_max / accel
    d_acc = accel * t_acc ** 2 / 2               # distance covered while speeding up
    if 2 * d_acc >= distance:                    # never reaches v_max: a triangle
        t_acc = math.sqrt(distance / accel)
        return "triangle", t_acc, 0.0, 2 * t_acc, accel * t_acc
    t_cruise = (distance - 2 * d_acc) / v_max
    return "trapezoid", t_acc, t_cruise, 2 * t_acc + t_cruise, v_max

for d in (1.0, 0.25, 0.1):
    shape, t_acc, t_cruise, total, peak = trapezoid(d, v_max=0.5, accel=1.0)
    print(f"{d:4} m: {shape:9}  accelerate {t_acc:.3f} s  cruise {t_cruise:.3f} s"
          f"  total {total:.3f} s  peak {peak:.3f} m/s")

 1.0 m: trapezoid  accelerate 0.500 s  cruise 1.500 s  total 2.500 s  peak 0.500 m/s
0.25 m: triangle   accelerate 0.500 s  cruise 0.000 s  total 1.000 s  peak 0.500 m/s
 0.1 m: triangle   accelerate 0.316 s  cruise 0.000 s  total 0.632 s  peak 0.316 m/s

The 0.25 m move sits exactly on the boundary: it just reaches 0.5 m/s and brakes at once. A trapezoidal profile still has sudden changes in acceleration at its corners, which shake the machine and the load. Many controllers therefore offer S-curve (jerk-limited) profiles, which round off the corners at the cost of a slightly longer move.

PID on a motor

The PID controller is the same on a robot as in a process plant: its output is the sum of a proportional, an integral and a derivative term of the error e (setpoint minus measurement):

u(t) = K_p \, e(t) + K_i \int_0^t e(\tau) \, d\tau + K_d \, \frac{de(t)}{dt}

A robot's controller is a program that runs at a fixed period \Delta t, so the integral becomes a running sum (a Riemann sum) and the derivative a difference between two samples (see the difference quotient):

u_k = K_p \, e_k + K_i \sum_{j=1}^{k} e_j \, \Delta t + K_d \, \frac{e_k - e_{k-1}}{\Delta t}

What changes on a robot is the time scale: the water tank on the OT page responds in minutes, a motor in a fraction of a second. The example uses a simple motor model: the speed v follows the drive command u with a time constant \tau, \tau \, \dot v = K u - v, with K = 2, \tau = 0.5 s, a loop every 10 ms, and a drive that cannot give more than ±1. The target speed is 1.0.

# A speed loop on a simple motor model. The motor's speed follows the drive
# command u with a time constant: tau * dv/dt = K * u - v.
K, TAU = 2.0, 0.5           # speed per unit of command, and time constant (s)
DT = 0.01                   # the controller runs every 10 ms
SETPOINT = 1.0              # target speed


def run(kp, ki):
    speed, integral = 0.0, 0.0
    for step in range(1, 301):
        error = SETPOINT - speed                 # e_k
        integral += error * DT                   # the sum of e * dt
        u = kp * error + ki * integral
        u = max(-1.0, min(1.0, u))               # the drive's limit: +-1
        speed += (K * u - speed) / TAU * DT      # the motor responds
        if step in (10, 50, 100, 300):
            print(f"  t={step * DT:.2f} s  speed {speed:.4f}  u {u:.4f}")


print("P  (Kp=2)")
run(kp=2.0, ki=0.0)
print("PI (Kp=2, Ki=4)")
run(kp=2.0, ki=4.0)

P  (Kp=2)
  t=0.10 s  speed 0.3659  u 1.0000
  t=0.50 s  speed 0.7931  u 0.4154
  t=1.00 s  speed 0.8000  u 0.4001
  t=3.00 s  speed 0.8000  u 0.4000
PI (Kp=2, Ki=4)
  t=0.10 s  speed 0.3659  u 1.0000
  t=0.50 s  speed 1.0347  u 0.5659
  t=1.00 s  speed 1.0268  u 0.5013
  t=3.00 s  speed 1.0005  u 0.5000

Reading the output:

In the first 0.1 s both controllers ask for more than the drive can give, so the command sits at its limit of 1.0 and the motor speeds up the same way.
P alone settles at 0.8. To hold a speed, the motor needs a command of speed / K, and P only gives a command while an error remains: here the balance is v = K K_p (1 - v), so v = 4/5, an error of 0.2 forever.
PI removes that error: the sum keeps growing while any error remains, until the command is the 0.5 that a speed of 1.0 needs. The price is an overshoot to about 1.04, 0.6 s after the start.

The derivative term brakes early when the error shrinks fast, but it also amplifies measurement noise: a small jitter in the encoder reading divided by a short \Delta t is a large number. Real controllers filter the D term, and many motor speed loops use PI only. The simulation below lets you see that effect.

A fixed loop rate

The discrete formula assumes that every step takes the same \Delta t. If the loop runs late now and then, the integral and the derivative are computed with the wrong time step and the tuning no longer holds. That is why motion control runs on a timer interrupt, a real-time operating system or a dedicated drive, not in a loop that waits for whatever else the computer is doing. The simulation below has the same problem in miniature: a browser draws 60 or 144 frames per second, or fewer when it is busy, so the controller runs in fixed steps of 1/50 s regardless of the frame rate (compare delta time in the Vibe Coding game).

Integral windup

Every real actuator has a limit: a maximum voltage, current or wheel speed. While the controller's output is stuck at that limit, the error stays large and the integral keeps adding it up, even though the extra output cannot be delivered. That is integral windup. When the error finally changes sign, the oversized integral first has to "unwind", and until it has, the controller keeps pushing in the old direction: a large overshoot, sometimes an oscillation.

The fix is called anti-windup, and the OT page's tank example uses it too. Common forms:

Conditional integration: stop adding to the integral while the output is at its limit and the error would push it further.
Clamping the I term to a fixed range, so it can never grow beyond a sensible share of the output.
Back-calculation: feed the difference between the wanted and the delivered output back into the integral, so it tracks what the actuator can really do.

The same PI loop as above, with and without conditional integration:

# A PI speed loop on a simple motor model. The motor's speed follows the
# drive command u with a time constant: tau * dv/dt = K * u - v.
K, TAU = 2.0, 0.5           # gain (speed per unit of command) and time constant (s)
DT = 0.01                   # the loop runs every 10 ms
KP, KI = 2.0, 4.0
SETPOINT = 1.0              # target speed


def run(anti_windup):
    speed, integral, peak = 0.0, 0.0, 0.0
    for step in range(1, 301):
        error = SETPOINT - speed
        new_integral = integral + error * DT
        raw = KP * error + KI * new_integral
        u = max(-1.0, min(1.0, raw))             # the drive's limit: +-1
        if not anti_windup or u == raw:          # anti-windup: freeze I at a limit
            integral = new_integral
        speed += (K * u - speed) / TAU * DT
        peak = max(peak, speed)
        if step in (10, 50, 100, 300):
            print(f"  t={step * DT:.2f} s  speed {speed:.4f}  u {u:.4f}")
    print(f"  peak {peak:.4f}")


print("without anti-windup")
run(anti_windup=False)
print("with anti-windup")
run(anti_windup=True)

without anti-windup
  t=0.10 s  speed 0.3659  u 1.0000
  t=0.50 s  speed 1.0347  u 0.5659
  t=1.00 s  speed 1.0268  u 0.5013
  t=3.00 s  speed 1.0005  u 0.5000
  peak 1.0440
with anti-windup
  t=0.10 s  speed 0.3659  u 1.0000
  t=0.50 s  speed 0.8989  u 0.5247
  t=1.00 s  speed 0.9677  u 0.4999
  t=3.00 s  speed 0.9994  u 0.5000
  peak 0.9994

With anti-windup the integral does not build up during the first 0.15 seconds at full command, so the speed approaches 1.0 without overshooting, at the cost of a slower last few percent. Which matters more, no overshoot or speed, depends on the machine; on a robot arm near a fixture, overshoot can mean a collision. The simulation below uses both conditional integration and a cap on the I term.

Try it: tune a line follower

The simulation below is a two-wheeled robot driving along a black tape line at 0.5 m/s, seen from above, with a PID controller that steers it. A sensor 0.15 m ahead of the wheels measures how far the line is to the side (the error, positive when the line is to the left); the controller turns that into a turn rate, and the turn rate into two wheel speeds (see Mobile Robots and Odometry for how two wheels steer). It is a small model, not a real robot: no wheel slip, no motor dynamics, and a perfect line.

How to use it

Press Play first. The keys D, R and P only reach the simulation after you click in the frame once or Tab to the drawing. The frame follows your browser's light or dark mode, not the theme switch of this site. Below the view, a plot shows the error over the last 12 seconds.

Mouse or touch	Keyboard
Sliders Kp (0–10), Ki (0–3) and Kd (0–5)	The sliders work with the arrow keys, Page Up/Down and Home/End as usual
Presets P, PD and PID (set the sliders and restart); checkboxes Uneven wheels (drift) and Sensor noise	All buttons and checkboxes can be reached with Tab
Buttons Disturb (a push of 0.15 m and a twist of 0.2 rad, alternately left and right, marked in the plot), Pause/Run and Reset	On the drawing: D disturbs, R resets, P pauses

The readout shows the three terms, the turn rate (with "max" when the output is at its limit), the wheel speeds and, since the last reset or disturbance, the overshoot, the number of times the robot crossed the line and the steady-state error. The drawing needs WebGL; without it the frame shows a message instead.

<main class="sim">
  <div class="stage">
    <canvas
      id="view"
      tabindex="0"
      aria-label="Top view of a two-wheeled robot following a line. Keys: D disturbs, R resets, P pauses."
    ></canvas>
    <p id="status" class="status" aria-live="polite"></p>
    <canvas
      id="plot"
      role="img"
      aria-label="Error over the last 12 seconds; the readout gives the numbers."
    ></canvas>
  </div>

  <div class="panel">
    <div class="group">
      <label for="kp">Kp proportional <output id="kp-out">4.0</output></label>
      <input type="range" id="kp" min="0" max="10" step="0.1" value="4" />
      <label for="ki">Ki integral <output id="ki-out">0.00</output></label>
      <input type="range" id="ki" min="0" max="3" step="0.05" value="0" />
      <label for="kd">Kd derivative <output id="kd-out">1.00</output></label>
      <input type="range" id="kd" min="0" max="5" step="0.05" value="1" />
    </div>

    <div class="buttons" aria-label="Presets">
      <button id="preset-p" type="button">P</button>
      <button id="preset-pd" type="button">PD</button>
      <button id="preset-pid" type="button">PID</button>
    </div>

    <div class="group">
      <label class="check"
        ><input type="checkbox" id="drift" checked /> Uneven wheels
        (drift)</label
      >
      <label class="check"
        ><input type="checkbox" id="noise" /> Sensor noise</label
      >
    </div>

    <div class="buttons">
      <button id="disturb" type="button">Disturb</button>
      <button id="pause" type="button">Pause</button>
      <button id="reset" type="button">Reset</button>
    </div>

    <pre id="readout" class="readout" aria-live="off"></pre>
  </div>
</main>

/* All colors in one place. script.js reads --paper, --ink, --muted,
   --grid, --soft and --accent from here, so the drawing follows dark mode too. */
:root {
  --paper: #ffffff;
  --ink: #111111;
  --muted: #666666;
  --grid: #e4e4e4;
  --soft: #b8b8b8;
  --accent: #677449;
  --font: system-ui, -apple-system, "Segoe UI", sans-serif;
  --mono: ui-monospace, "SFMono-Regular", Menlo, Consolas, monospace;
  color-scheme: light;
}

@media (prefers-color-scheme: dark) {
  :root {
    --paper: #161616;
    --ink: #ededed;
    --muted: #a3a3a3;
    --grid: #2a2a2a;
    --soft: #5a5a5a;
    --accent: #a6b082;
    color-scheme: dark;
  }
}

html,
body {
  height: 100%;
  margin: 0;
  background: var(--paper);
  color: var(--ink);
  font: 13px/1.4 var(--font);
}

/* Drawing on the left, controls on the right; stacked on a phone. */
.sim {
  display: grid;
  grid-template-columns: minmax(0, 1fr) 15rem;
  height: 100%;
}

/* The robot on top, the error plot below it. */
.stage {
  position: relative;
  display: grid;
  grid-template-rows: minmax(0, 1fr) 34%;
  min-height: 0;
}

#plot {
  display: block;
  width: 100%;
  height: 100%;
  border-top: 1px solid var(--ink);
}

.buttons {
  display: flex;
  flex-wrap: wrap;
  gap: 6px;
}

.buttons button {
  flex: 1;
}

.check {
  display: flex;
  gap: 6px;
  align-items: center;
}

#view {
  display: block;
  min-height: 0;
  width: 100%;
  height: 100%;
}

#view:focus-visible {
  outline: 1px solid var(--ink);
  outline-offset: -4px;
}

.status {
  position: absolute;
  left: 12px;
  top: 8px;
  margin: 0;
  font: 12px/1.4 var(--mono);
  color: var(--accent);
  pointer-events: none;
}

.panel {
  display: flex;
  flex-direction: column;
  gap: 10px;
  padding: 12px 14px;
  overflow-y: auto;
  border-left: 1px solid var(--ink);
}

.group {
  display: grid;
  gap: 4px;
}

.group label:not(.check) {
  display: flex;
  justify-content: space-between;
}

output {
  font-family: var(--mono);
}

input[type="range"] {
  width: 100%;
  margin: 0 0 6px;
  accent-color: var(--accent);
}

input[type="checkbox"] {
  accent-color: var(--accent);
}

button {
  padding: 5px 10px;
  font: inherit;
  color: var(--ink);
  background: transparent;
  border: 1px solid var(--ink);
  border-radius: 0;
  cursor: pointer;
}

button:hover {
  background: var(--grid);
}

button:focus-visible,
input:focus-visible {
  outline: 1px solid var(--accent);
  outline-offset: 2px;
}

.readout {
  margin: 0;
  padding: 8px;
  font: 12px/1.5 var(--mono);
  white-space: pre;
  border: 1px solid var(--grid);
}

.note {
  margin: 0;
  padding: 24px;
  color: var(--muted);
}

@media (max-width: 560px) {
  .sim {
    grid-template-columns: 1fr;
    grid-template-rows: minmax(300px, 70vh) auto;
    height: auto;
  }

  .panel {
    border-left: 0;
    border-top: 1px solid var(--ink);
  }
}

import * as THREE from "three";

// ---- ROBOT MATH (pure functions: no Three.js, no DOM) ----
// A two-wheeled (differential-drive) robot drives along a line on the
// floor (the x-axis) at a steady speed. A PID controller steers it: the
// error is how far the line is, sideways, from the sensor at the front.
// Units: meters, seconds, radians. Error > 0: the line is to the left.

const SPEED = 0.5; // forward speed, m/s
const TRACK = 0.3; // distance between the two wheels, m
const SENSOR_AHEAD = 0.15; // the line sensor sits this far ahead, m
const WHEEL_MAX = 1.0; // the fastest a wheel can turn, m/s
const MAX_TURN = (2 * (WHEEL_MAX - SPEED)) / TRACK; // rad/s at full speed
const DRIFT = 0.4; // rad/s: one wheel slightly bigger pulls the robot left
const NOISE = 0.02; // m: standard deviation of the sensor noise
const I_LIMIT = 1.0; // rad/s: the I term never grows beyond this
const CONTROL_STEP = 1 / 50; // the controller runs 50 times a second
const DERIVATIVE_FILTER = 0.1; // s: smooths the D term against noise
const MAX_STEP = 0.05; // longest frame time we accept, in seconds
const START = { x: 0, y: -0.3, heading: 0 }; // 0.3 m right of the line
const KICK = { y: 0.15, heading: 0.2 }; // what Disturb does to the robot
const SAMPLE_EVERY = 2 * CONTROL_STEP; // seconds between plot points
const PLOT_SECONDS = 12;
const PLOT_RANGE = 0.4; // the plot shows -0.4 .. +0.4 m
const SETTLE_WINDOW = 2; // s: steady-state error = mean of the last 2 s
const SETTLE_AFTER = 8; // s: only report it after this long
const BAND = 0.01; // m: a crossing counts once the error is past this
const PRESETS = {
  p: { kp: 4, ki: 0, kd: 0 },
  pd: { kp: 4, ki: 0, kd: 2.5 },
  pid: { kp: 4, ki: 1, kd: 2.5 },
};

function clamp(value, low, high) {
  return Math.min(high, Math.max(low, value));
}

/** A fresh controller: nothing summed yet, no previous error. */
function createPid() {
  return { integral: 0, previous: null, derivative: 0 };
}

/**
 * One controller update. Output = Kp·e + Ki·∫e dt + Kd·de/dt, limited to
 * ±limit. Anti-windup: while the output is stuck at its limit, the integral
 * stops growing in that direction, and the I term stays within ±I_LIMIT.
 * The derivative is low-pass filtered, as in most real controllers.
 * Returns the output, the three terms and the new controller state.
 */
function pidStep(pid, error, dt, gains, limit = MAX_TURN, antiWindup = true) {
  const change = pid.previous === null ? 0 : (error - pid.previous) / dt;
  const share = dt / (DERIVATIVE_FILTER + dt);
  const derivative = pid.derivative + (change - pid.derivative) * share;
  let integral = pid.integral + error * dt;
  const p = gains.kp * error;
  const d = gains.kd * derivative;
  const raw = p + gains.ki * integral + d;
  if (antiWindup) {
    const pushingFurther = Math.sign(error) === Math.sign(raw);
    if (Math.abs(raw) > limit && pushingFurther) integral = pid.integral;
    const most = gains.ki > 0 ? I_LIMIT / gains.ki : Infinity;
    integral = clamp(integral, -most, most);
  }
  const terms = { p, i: gains.ki * integral, d };
  const output = clamp(terms.p + terms.i + terms.d, -limit, limit);
  return {
    output,
    saturated: Math.abs(terms.p + terms.i + terms.d) > limit,
    terms,
    pid: { integral, previous: error, derivative },
  };
}

/** Wheel speeds for a forward speed and a turn rate (left turn > 0). */
function wheelSpeeds(speed, turn, track = TRACK, max = WHEEL_MAX) {
  return {
    left: clamp(speed - (turn * track) / 2, -max, max),
    right: clamp(speed + (turn * track) / 2, -max, max),
  };
}

/** Moves the robot for dt seconds with these wheel speeds (plus drift). */
function stepRobot(robot, wheels, dt, drift = 0, track = TRACK) {
  const speed = (wheels.left + wheels.right) / 2;
  const turn = (wheels.right - wheels.left) / track + drift;
  const middle = robot.heading + (turn * dt) / 2; // heading halfway the step
  return {
    x: robot.x + speed * Math.cos(middle) * dt,
    y: robot.y + speed * Math.sin(middle) * dt,
    heading: robot.heading + turn * dt,
  };
}

/** A normally distributed number (mean 0, deviation 1) from random(). */
function gaussian(random) {
  const u = 1 - random(); // never 0, so the log is finite
  return Math.sqrt(-2 * Math.log(u)) * Math.cos(2 * Math.PI * random());
}

/** The sensor's sideways distance to the line (> 0: line on the left). */
function sensorError(robot, ahead = SENSOR_AHEAD) {
  const sensorY = robot.y + ahead * Math.sin(robot.heading);
  return 0 - sensorY; // set point (the line, y = 0) minus position
}

/** One control step: measure, control, drive. */
function controlStep(state, gains, options, random = Math.random) {
  const noise = options.noise ? NOISE * gaussian(random) : 0;
  const error = sensorError(state.robot);
  const measured = error - noise;
  const control = pidStep(state.pid, measured, CONTROL_STEP, gains);
  const wheels = wheelSpeeds(SPEED, control.output);
  const drift = options.drift ? DRIFT : 0;
  return {
    robot: stepRobot(state.robot, wheels, CONTROL_STEP, drift),
    pid: control.pid,
    time: state.time + CONTROL_STEP,
    error,
    measured,
    wheels,
    terms: control.terms,
    turn: control.output,
    saturated: control.saturated,
  };
}

/** How many fixed steps fit in the time we have, and what is left over. */
function fixedSteps(saved, dt, step = CONTROL_STEP) {
  const total = saved + dt;
  const steps = Math.floor(total / step + 1e-9);
  return { steps, rest: Math.max(0, total - steps * step) };
}

/**
 * Overshoot, crossings and steady-state error of a response (errors
 * sampled every sampleDt seconds since the last reset or disturbance).
 */
function responseMetrics(values, sampleDt = SAMPLE_EVERY) {
  const result = { overshoot: null, ratio: null, crossings: 0, steady: null };
  if (values.length < 2) return result;
  const initial = values[0];
  const first = Math.sign(initial);
  let side = first; // the side we were last clearly on
  let worst = 0;
  for (const v of values) {
    if (Math.abs(v) > BAND && Math.sign(v) !== side) {
      result.crossings++;
      side = Math.sign(v);
    }
    if (result.crossings > 0) worst = Math.max(worst, -first * v);
  }
  if (first !== 0) {
    result.overshoot = worst;
    result.ratio = worst / Math.abs(initial);
  }
  if (values.length * sampleDt >= SETTLE_AFTER) {
    const last = values.slice(-Math.round(SETTLE_WINDOW / sampleDt));
    result.steady = last.reduce((sum, v) => sum + v, 0) / last.length;
  }
  return result;
}

/** Plot points (x, y, ...) for values; the newest sits at the right edge. */
function plotPoints(values, width, height, capacity, range = PLOT_RANGE) {
  const points = [];
  const offset = capacity - values.length;
  values.forEach((v, i) => {
    const x = (width * (i + offset)) / (capacity - 1);
    const y = height / 2 - (clamp(v, -range, range) / range) * (height / 2);
    points.push(x, y);
  });
  return points;
}

/** An orthographic view of ±half meters up and down around (0, 0). */
function fitView(width, height, half = 0.6) {
  const aspect = width / Math.max(height, 1);
  return {
    left: -half * aspect,
    right: half * aspect,
    top: half,
    bottom: -half,
  };
}

const signed = (v, digits) => (v >= 0 ? "+" : "") + v.toFixed(digits);

/** The text of the readout panel. */
function readoutText(s, metrics) {
  const m = (v) => (v === null ? "  …" : signed(v, 3));
  const percent =
    metrics.ratio === null ? "" : ` (${Math.round(metrics.ratio * 100)}%)`;
  return [
    `time      ${s.time.toFixed(1)} s`,
    `error     ${signed(s.error, 3)} m`,
    `heading   ${signed((s.robot.heading * 180) / Math.PI, 1)}°`,
    `P ${signed(s.terms.p, 2)}  I ${signed(s.terms.i, 2)}`,
    `D ${signed(s.terms.d, 2)}`,
    `turn      ${signed(s.turn, 2)} rad/s${s.saturated ? " max" : ""}`,
    `wheels    ${s.wheels.left.toFixed(2)} / ${s.wheels.right.toFixed(2)} m/s`,
    `since reset or disturb:`,
    `overshoot ${metrics.overshoot === null ? "  …" : metrics.overshoot.toFixed(3)} m${percent}`,
    `crossings ${metrics.crossings}`,
    `steady    ${m(metrics.steady)} m`,
  ].join("\n");
}
// ---- END OF ROBOT MATH ----

// ---- Page elements ----
const $ = (id) => document.getElementById(id);
const canvas = $("view");
const plot = $("plot");
const statusLine = $("status");
const gainInputs = { kp: $("kp"), ki: $("ki"), kd: $("kd") };
const digits = { kp: 1, ki: 2, kd: 2 };

// ---- State ----
const PLOT_CAPACITY = Math.round(PLOT_SECONDS / SAMPLE_EVERY);
const TRAIL_POINTS = 900;
let state;
let saved = 0; // physics time not used yet
let steps = 0; // control steps since the start
let sampleClock = 0;
let samples; // { error: [], measured: [], marks: [] } for the plot
let response; // errors since the last reset or disturbance
let kicks = 0;
let paused = false;
let turnedAround = false; // stays true until Reset
let view = fitView(1, 1);

function gains() {
  const g = {};
  for (const key in gainInputs) g[key] = Number(gainInputs[key].value);
  return g;
}

function options() {
  return { drift: $("drift").checked, noise: $("noise").checked };
}

// ---- Renderer, scene and camera ----
let renderer;
try {
  renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
} catch (error) {
  statusLine.textContent = "WebGL is not available, so nothing can be drawn.";
  throw error;
}
renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
const scene = new THREE.Scene();
const camera = new THREE.OrthographicCamera(-1, 1, 1, -1, -10, 10);
const ctx = plot.getContext("2d");

const mat = {
  grid: new THREE.LineBasicMaterial(),
  ink: new THREE.LineBasicMaterial(),
  inkFill: new THREE.MeshBasicMaterial(),
  paper: new THREE.MeshBasicMaterial(),
  accent: new THREE.LineBasicMaterial(),
};
let colors = {};

function readColors() {
  const style = getComputedStyle(document.documentElement);
  const pick = (name, fallback) =>
    style.getPropertyValue(name).trim() || fallback;
  return {
    paper: pick("--paper", "#ffffff"),
    ink: pick("--ink", "#111111"),
    muted: pick("--muted", "#666666"),
    grid: pick("--grid", "#e4e4e4"),
    soft: pick("--soft", "#b8b8b8"),
    accent: pick("--accent", "#b5441f"),
  };
}

function applyColors() {
  colors = readColors();
  scene.background = new THREE.Color(colors.paper);
  mat.grid.color.set(colors.grid);
  mat.ink.color.set(colors.ink);
  mat.inkFill.color.set(colors.ink);
  mat.paper.color.set(colors.paper);
  mat.accent.color.set(colors.accent);
  drawPlot();
}

function lineFrom(points, material, closed = false) {
  const geometry = new THREE.BufferGeometry();
  const xyz = [];
  for (let i = 0; i < points.length; i += 2)
    xyz.push(points[i], points[i + 1], 0);
  geometry.setAttribute("position", new THREE.Float32BufferAttribute(xyz, 3));
  return closed
    ? new THREE.LineLoop(geometry, material)
    : new THREE.Line(geometry, material);
}

// ---- The floor: a 10 cm grid that moves along with the camera ----
const gridPoints = [];
for (let v = -8; v <= 8.001; v += 0.1) gridPoints.push(v, -3, 0, v, 3, 0);
for (let v = -3; v <= 3.001; v += 0.1) gridPoints.push(-8, v, 0, 8, v, 0);
const gridGeometry = new THREE.BufferGeometry();
gridGeometry.setAttribute(
  "position",
  new THREE.Float32BufferAttribute(gridPoints, 3),
);
const grid = new THREE.LineSegments(gridGeometry, mat.grid);
scene.add(grid);

// The line on the floor: a strip of black tape, 2 cm wide.
const tape = new THREE.Mesh(new THREE.PlaneGeometry(20, 0.02), mat.inkFill);
tape.position.z = 0.1;
scene.add(tape);

// ---- The trail: where the robot has been ----
const trailPositions = new Float32Array(TRAIL_POINTS * 3);
const trailGeometry = new THREE.BufferGeometry();
trailGeometry.setAttribute(
  "position",
  new THREE.BufferAttribute(trailPositions, 3),
);
const trail = new THREE.Line(trailGeometry, mat.accent);
trail.frustumCulled = false; // the points change every frame
trail.position.z = 0.2;
scene.add(trail);
let trailCount = 0;

// ---- The robot (seen from above, pointing along +x) ----
const robotMesh = new THREE.Group();
robotMesh.name = "robot";
const body = [-0.12, -0.1, 0.12, -0.1, 0.12, 0.1, -0.12, 0.1];
robotMesh.add(new THREE.Mesh(new THREE.PlaneGeometry(0.24, 0.2), mat.paper));
robotMesh.add(lineFrom(body, mat.ink, true));
for (const side of [-1, 1]) {
  const wheel = new THREE.Mesh(new THREE.PlaneGeometry(0.1, 0.03), mat.inkFill);
  wheel.position.set(-0.03, side * (TRACK / 2), 0.01);
  robotMesh.add(wheel);
}
// The line sensor: a short bar on a stalk at the front.
const ahead = SENSOR_AHEAD;
robotMesh.add(lineFrom([0.12, 0, ahead, 0], mat.ink));
robotMesh.add(lineFrom([ahead, -0.06, ahead, 0.06], mat.ink));
robotMesh.position.z = 0.3;
scene.add(robotMesh);

// A dimension line from the sensor to the line: the error, drawn.
const dimension = lineFrom([0, 0, 0, 0, -0.03, 0, 0.03, 0], mat.accent);
dimension.name = "error-line";
dimension.position.z = 0.4;
scene.add(dimension);
applyColors();

// ---- Fit both canvases, also after a resize ----
function resize() {
  const width = canvas.clientWidth || window.innerWidth;
  const height = canvas.clientHeight || window.innerHeight;
  renderer.setSize(width, height, false);
  view = fitView(width, height);
  Object.assign(camera, view);
  camera.updateProjectionMatrix();
  const ratio = Math.min(window.devicePixelRatio, 2);
  plot.width = Math.max(1, Math.round((plot.clientWidth || width) * ratio));
  plot.height = Math.max(1, Math.round((plot.clientHeight || 120) * ratio));
  drawPlot();
}

// ---- The plot (2D canvas): error over the last 12 seconds ----
function drawPlot() {
  if (!samples || !ctx) return;
  const ratio = Math.min(window.devicePixelRatio, 2);
  const w = plot.width;
  const h = plot.height;
  ctx.setTransform(1, 0, 0, 1, 0, 0);
  ctx.fillStyle = colors.paper;
  ctx.fillRect(0, 0, w, h);
  ctx.lineWidth = ratio; // 1 CSS pixel: a hairline
  const hline = (value, color) => {
    const y = h / 2 - (value / PLOT_RANGE) * (h / 2);
    ctx.strokeStyle = color;
    ctx.beginPath();
    ctx.moveTo(0, y);
    ctx.lineTo(w, y);
    ctx.stroke();
  };
  for (const v of [-0.3, -0.2, -0.1, 0.1, 0.2, 0.3]) hline(v, colors.grid);
  // Seconds lines that scroll with time, every 2 s.
  const now = state.time;
  ctx.strokeStyle = colors.grid;
  for (let t = Math.ceil((now - PLOT_SECONDS) / 2) * 2; t <= now; t += 2) {
    const x = w * (1 - (now - t) / PLOT_SECONDS);
    ctx.beginPath();
    ctx.moveTo(x, 0);
    ctx.lineTo(x, h);
    ctx.stroke();
  }
  hline(0, colors.soft);
  // Dashed marks where Disturb was pressed.
  ctx.setLineDash([4 * ratio, 4 * ratio]);
  ctx.strokeStyle = colors.ink;
  for (const mark of samples.marks) {
    const x =
      (w * (mark - (sampleClock - PLOT_CAPACITY) - 1)) / (PLOT_CAPACITY - 1);
    ctx.beginPath();
    ctx.moveTo(x, 0);
    ctx.lineTo(x, h);
    ctx.stroke();
  }
  ctx.setLineDash([]);
  const polyline = (values, color) => {
    const p = plotPoints(values, w, h, PLOT_CAPACITY);
    ctx.strokeStyle = color;
    ctx.beginPath();
    for (let i = 0; i < p.length; i += 2) ctx.lineTo(p[i], p[i + 1]);
    ctx.stroke();
  };
  if (options().noise) polyline(samples.measured, colors.soft);
  polyline(samples.error, colors.accent);
  ctx.fillStyle = colors.muted;
  ctx.font = `${10 * ratio}px ui-monospace, Menlo, Consolas, monospace`;
  ctx.fillText("error (m), last 12 s", 6 * ratio, 13 * ratio);
  ctx.fillText("+0.4", w - 34 * ratio, 13 * ratio);
  ctx.fillText("-0.4", w - 34 * ratio, h - 5 * ratio);
}

window.addEventListener("resize", resize);
window
  .matchMedia("(prefers-color-scheme: dark)")
  .addEventListener("change", applyColors);

// ---- Start, reset, disturb ----
function reset() {
  state = {
    robot: { ...START },
    pid: createPid(),
    time: 0,
    error: sensorError(START),
    measured: sensorError(START),
    wheels: { left: SPEED, right: SPEED },
    terms: { p: 0, i: 0, d: 0 },
    turn: 0,
    saturated: false,
  };
  saved = 0;
  steps = 0;
  sampleClock = 0;
  samples = { error: [], measured: [], marks: [] };
  response = [];
  trailCount = 0;
  kicks = 0;
  turnedAround = false;
}

function disturb() {
  // A push sideways and a twist, alternately to the left and the right.
  const side = kicks % 2 === 0 ? 1 : -1;
  kicks++;
  const r = state.robot;
  state.robot = {
    x: r.x,
    y: r.y + side * KICK.y,
    heading: r.heading + side * KICK.heading,
  };
  samples.marks.push(sampleClock);
  response = [];
}

function setPaused(on) {
  paused = on;
  $("pause").textContent = on ? "Run" : "Pause";
}

function showGains() {
  for (const key in gainInputs) {
    $(key + "-out").textContent = Number(gainInputs[key].value).toFixed(
      digits[key],
    );
  }
}

for (const key in gainInputs)
  gainInputs[key].addEventListener("input", showGains);
for (const name of Object.keys(PRESETS)) {
  $("preset-" + name).addEventListener("click", () => {
    for (const key in gainInputs)
      gainInputs[key].value = String(PRESETS[name][key]);
    showGains();
    reset(); // a fresh start shows the step response of the new gains
  });
}
$("noise").addEventListener("change", drawPlot);
$("disturb").addEventListener("click", disturb);
$("pause").addEventListener("click", () => setPaused(!paused));
$("reset").addEventListener("click", reset);
// Letters by what the key types (event.key), so they work on every layout.
canvas.addEventListener("keydown", (event) => {
  const key = event.key.toLowerCase();
  if (key === "d") disturb();
  else if (key === "r") reset();
  else if (key === "p") setPaused(!paused);
});

// ---- The animation loop ----
const timer = new THREE.Timer();
timer.connect(document);
let lastStatus = "";
let lastReadout = "";

function addSample() {
  sampleClock++;
  const push = (list, value, max) => {
    list.push(value);
    if (list.length > max) list.shift();
  };
  push(samples.error, state.error, PLOT_CAPACITY);
  push(samples.measured, state.measured, PLOT_CAPACITY);
  push(response, state.error, 60 / SAMPLE_EVERY);
  samples.marks = samples.marks.filter((m) => m > sampleClock - PLOT_CAPACITY);
  if (trailCount === TRAIL_POINTS) {
    trailPositions.copyWithin(0, 3); // drop the oldest point
    trailCount--;
  }
  trailPositions.set([state.robot.x, state.robot.y, 0], trailCount * 3);
  trailCount++;
}

/** Runs the fixed steps that fit in dt; true when the plot got new points. */
function simulate(dt) {
  const run = fixedSteps(saved, dt);
  saved = run.rest;
  const g = gains();
  const o = options();
  let added = false;
  for (let i = 0; i < run.steps; i++) {
    state = controlStep(state, g, o);
    steps++;
    if (steps % 2 === 0) {
      addSample(); // every other step: SAMPLE_EVERY
      added = true;
    }
  }
  return added;
}

function draw() {
  const { x, y, heading } = state.robot;
  robotMesh.position.set(x, y, robotMesh.position.z);
  robotMesh.rotation.z = heading;
  const sx = x + SENSOR_AHEAD * Math.cos(heading);
  const sy = y + SENSOR_AHEAD * Math.sin(heading);
  const p = dimension.geometry.attributes.position;
  p.setXY(0, sx, sy);
  p.setXY(1, sx, 0);
  p.setXY(2, sx - 0.03, 0);
  p.setXY(3, sx + 0.03, 0);
  p.needsUpdate = true;
  dimension.geometry.computeBoundingSphere();
  trailGeometry.setDrawRange(0, trailCount);
  trailGeometry.attributes.position.needsUpdate = true;

  // The camera follows the robot, which stays a third from the left.
  camera.position.x = x + (view.right - view.left) / 6;
  grid.position.x = Math.round(camera.position.x / 0.1) * 0.1;
  tape.position.x = camera.position.x;

  let status = "";
  if (Math.cos(heading) < 0) turnedAround = true;
  if (turnedAround) status = "It turned around: unstable gains. Reset.";
  else if (Math.abs(y) > 0.6) status = "Far off the line: try other gains.";
  if (status !== lastStatus) statusLine.textContent = lastStatus = status;
  const text = readoutText(state, responseMetrics(response));
  if (text !== lastReadout) $("readout").textContent = lastReadout = text;
}

reset();
showGains();
resize();
renderer.setAnimationLoop((time) => {
  timer.update(time);
  const dt = Math.min(timer.getDelta(), MAX_STEP);
  if (!paused && simulate(dt)) drawPlot();
  draw();
  renderer.render(scene, camera);
});

What to try

Keep Uneven wheels on and press the presets P, PD and PID in turn. The drift is a constant pull to the left of 0.4 rad/s. With P (Kp 4) the robot swings across the line and back (an overshoot of 115% of the starting distance) and then settles 0.10 m beside it. PD damps the swing (50%) but settles at the same −0.10 m. PID overshoots a little more than PD (60%) and ends on the line.
Why −0.10 m? Without an I term the controller can only cancel the drift with a standing error: the turn rate K_p e must equal −0.4 rad/s, so e = -0.4 / K_p. Set Kp to 8 with P only and the steady error halves. Turn the drift off and it disappears, even without an I term.
Turn on Sensor noise (random errors with a standard deviation of 2 cm, drawn as a gray line in the plot) and raise Kd. The D term in the readout jumps around much more than the P term, and so does the turn command: the derivative amplifies noise, even with the filter the simulation puts on it.
Press Disturb a few times while it drives. Compare how fast P, PD and PID recover, and where the robot ends up each time.
Try unstable gains, such as Kp 0.5, Ki 3 and Kd 0. The integral dominates, the swings grow, and the robot can end up driving the wrong way; the status line then says "It turned around: unstable gains. Reset."

The code below the frame is the complete simulation (HTML, CSS and JavaScript), ready to paste into a CodePen Pen; the HTML goes inside <body>. Its "ROBOT MATH" section at the top holds the pure functions this page explains: pidStep (with the derivative filter and anti-windup), wheelSpeeds, stepRobot, sensorError and responseMetrics; the Python version below mirrors them. It imports Three.js as import * as THREE from "three"; CodePen 2.0 adds Three.js for you. The simulation was built against Three.js r186 (three@0.186.1, as of October 2026). For CodePen's Classic editor, see If you use the Classic editor.

The same controller in Python

A Python version of the simulation's controller and robot model, without the drawing: the controller runs 50 times a second for 30 seconds from 0.3 m beside the line, with the drift on, and measures the overshoot and the mean error of the last two seconds the way the readout does. The turn rate is limited to what the wheels allow: with a forward speed of 0.5 m/s, a track of 0.3 m (a larger robot than the 0.16 m example on Mobile Robots and Odometry) and at most 1.0 m/s per wheel, that is 2 (1.0 - 0.5) / 0.3 \approx 3.33 rad/s.

# The line follower of the simulation, in Python: same model, same numbers.
import math

SPEED, TRACK, SENSOR_AHEAD = 0.5, 0.3, 0.15    # m/s, m, m
WHEEL_MAX = 1.0                                # m/s per wheel
MAX_TURN = 2 * (WHEEL_MAX - SPEED) / TRACK     # rad/s the wheels allow
DRIFT = 0.4                                    # rad/s pull to the left
I_LIMIT = 1.0                                  # rad/s cap on the I term
STEP = 1 / 50                                  # the controller runs at 50 Hz
FILTER = 0.1                                   # s, low-pass on the D term


def clamp(v, low, high):
    return min(high, max(low, v))


def sign(v):
    return (v > 0) - (v < 0)


def pid_step(pid, error, gains):
    """One update: returns the turn rate and the new controller state."""
    kp, ki, kd = gains
    integral, previous, derivative = pid
    change = 0.0 if previous is None else (error - previous) / STEP
    derivative += (change - derivative) * STEP / (FILTER + STEP)
    new_integral = integral + error * STEP
    raw = kp * error + ki * new_integral + kd * derivative
    if abs(raw) > MAX_TURN and sign(error) == sign(raw):
        new_integral = integral                # anti-windup: stop integrating
    if ki > 0:
        new_integral = clamp(new_integral, -I_LIMIT / ki, I_LIMIT / ki)
    out = kp * error + ki * new_integral + kd * derivative
    return clamp(out, -MAX_TURN, MAX_TURN), (new_integral, error, derivative)


def drive(robot, turn):
    """Two wheel speeds for the turn rate, then move the robot one step."""
    x, y, heading = robot
    left = clamp(SPEED - turn * TRACK / 2, -WHEEL_MAX, WHEEL_MAX)
    right = clamp(SPEED + turn * TRACK / 2, -WHEEL_MAX, WHEEL_MAX)
    speed = (left + right) / 2
    rate = (right - left) / TRACK + DRIFT
    middle = heading + rate * STEP / 2
    return (x + speed * math.cos(middle) * STEP,
            y + speed * math.sin(middle) * STEP,
            heading + rate * STEP)


def error_of(robot):
    x, y, heading = robot
    return -(y + SENSOR_AHEAD * math.sin(heading))   # > 0: the line is to the left


def follow(gains, seconds=30):
    robot = (0.0, -0.3, 0.0)                         # x, y, heading: 0.3 m right of the line
    pid = (0.0, None, 0.0)                           # integral, previous error, D term
    errors = []
    for step in range(1, round(seconds / STEP) + 1):
        error = error_of(robot)
        turn, pid = pid_step(pid, error, gains)
        robot = drive(robot, turn)
        if step % 2 == 0:
            errors.append(error)                     # sampled every 0.04 s
    start = errors[0]
    first_crossing = next(i for i, e in enumerate(errors) if e < -0.01)  # past a 1 cm band
    overshoot = max(-e for e in errors[first_crossing:])
    steady = sum(errors[-50:]) / 50                  # the last 2 s
    return overshoot / start, steady


PRESETS = [("default", (4, 0, 1)), ("P", (4, 0, 0)),
           ("PD", (4, 0, 2.5)), ("PID", (4, 1, 2.5))]           # (Kp, Ki, Kd)
for name, gains in PRESETS:
    ratio, steady = follow(gains)
    steady = round(steady, 3) + 0.0              # avoid printing -0.000
    print(f"{name:7}  overshoot {ratio:4.0%}  steady-state error {steady:+.3f} m")

default  overshoot  79%  steady-state error -0.100 m
P        overshoot 115%  steady-state error -0.100 m
PD       overshoot  50%  steady-state error -0.100 m
PID      overshoot  60%  steady-state error +0.000 m

The numbers match the presets in the simulation. Change the gains in the list and you can explore without a browser.

Cascade control in servo drives

One PID loop from position error straight to motor voltage is hard to tune: the loop would have to deal with the electrical behavior of the motor, the inertia of the arm and the position all at once. Servo drives use cascade control instead: loops nested inside each other, where the output of an outer loop is the setpoint of the next inner one.

Cascade control in a servo drive: the current loop is innermost and fastest, the position loop outermost and slowest.

The current loop (innermost) sets the motor current, and so the torque, since torque is proportional to current in a DC motor (see Motors and Drives).
The velocity loop asks the current loop for the torque that keeps the speed on target.
The position loop (outermost) asks the velocity loop for the speed that closes the position gap.

Each inner loop runs faster than the loop around it, so to the outer loop the inner one looks like an almost perfect actuator: the position loop can assume that the speed it asks for is the speed it gets. The loops are tuned from the inside out, current first; the outer loops depend on the load and the mechanics, so they are tuned on the machine itself.

Feedforward and following error

A feedback controller only acts once there is an error. When the trajectory is known in advance, that is a waste: the controller already knows the robot must move at 0.5 m/s, so why wait until it has fallen behind? Feedforward control adds the command that the plan predicts directly to the controller output: the planned speed to a speed command, the torque needed for the planned acceleration, or the torque that holds an arm up against gravity (gravity compensation). The feedback loop then only corrects what the prediction gets wrong.

Following error

The gap between where the trajectory says the axis should be and where it actually is, is the following error (also called tracking error). A P position loop on an axis that moves at a constant speed v needs a standing error to produce that speed command:

K_p \, e = v \quad\Rightarrow\quad e = \frac{v}{K_p}

In the model below (an axis that follows its speed command at once, a position loop at 1 kHz with K_p = 20 per second, a planned speed of 0.5 m/s), that is 0.5 / 20 = 0.025 m: the axis runs 25 mm behind the plan for as long as it moves. Adding the planned speed as feedforward removes it.

# A position loop drives an axis that follows its speed command at once.
# The reference moves at a steady 0.5 m/s: how far behind is the axis?
DT = 0.001                  # a 1 kHz position loop
KP = 20.0                   # 1/s: speed command per meter of error
V_REF = 0.5                 # m/s, the planned speed


def track(feedforward, seconds=1.0):
    reference = position = 0.0
    for _ in range(round(seconds / DT)):
        error = reference - position
        command = KP * error + (V_REF if feedforward else 0.0)
        position += command * DT                 # the axis moves
        reference += V_REF * DT                  # the plan moves on
    return reference - position                  # the following error


print(f"P only:             {track(False) * 1000:6.2f} mm behind")
print(f"P plus feedforward: {track(True) * 1000:6.2f} mm behind")
print(f"v / Kp:             {V_REF / KP * 1000:6.2f} mm")

P only:              25.00 mm behind
P plus feedforward:   0.00 mm behind
v / Kp:              25.00 mm

A real axis is not that ideal, so some following error remains, but feedforward typically shrinks it a great deal. Drives monitor it continuously: a following error above a set limit means the axis cannot keep up (a collision, a jammed mechanism, a failing motor or a wrong payload setting), and the drive stops the axis with a fault instead of pushing harder. When a robot stops with a following-error or "position deviation" alarm, the cause is often mechanical or a wrong load setting rather than the control software.