Control: Making It Do What You Want

Robotics Fundamentals

Course 1 · Chapter 7 · Control: Making It Do What You Want

Robotics Fundamentals 6 ended on an idea: a robot that only commands its motors is trusting that the world is exactly as it was when the commands were written, and a robot that measures what its motors actually did can correct itself. This chapter is about the correcting. Given a target (a speed, a heading, a position) and a measurement (the encoder speed or gyro heading of Robotics Fundamentals 6), a controller decides what to send to the motor next. We start from the simplest possible controller, on or off, and see why it chatters. We then build up the famous PID controller one term at a time (proportional, integral, derivative), simulating a motor speed loop step by step by hand so that every number can be checked. We look at why a position loop rings (the reason robots wobble), what a delay in the loop does to the largest gain you can use, how to tune a controller by hand and with the Ziegler-Nichols method (and what that method gets wrong), and finish with cascaded loops, feedforward, and two classic PID jobs: following a line and holding a heading.

The controller gains, motor constants, delays and noise figures in this chapter are illustrative or typical values chosen so that the arithmetic is easy to follow, not data for a particular motor. The formulas (the first-order step response, the steady-state error of proportional control, the discrete PID, the damping ratio, natural frequency and overshoot of a standard second-order system, and the Ziegler-Nichols table) are standard. Every simulation table in this chapter was recomputed step by step for the model stated next to it. The code is deliberately short, hardware-neutral Python-style pseudocode; real board code is the job of Programming for Robotics.

1. Open loop, closed loop, and the error

A plant is whatever the controller is trying to control: here, a motor with a wheel on it. The controller's output u goes into the plant (for a motor, a voltage, set by a PWM duty cycle) and the plant's response y comes out (a speed or an angle). There are two ways to arrange the loop.

OPEN LOOP: the controller works out u from the target alone and never looks at the result. target r --------> [ controller ] --- u --------> [ plant: motor + wheel ] --------> speed y CLOSED LOOP: the controller is told how far the result is from the target, and acts on the difference. target r ---->( + )-- e --> [ controller ] --- u ---> [ plant: motor + wheel ] ---+---> speed y ^ - | | | +----------------------- [ sensor: encoder ] <----------------------+ measurement error e = r - y (setpoint minus measurement: e is positive when the robot is too slow or too far behind)

The sign convention matters and is the same everywhere in this chapter: error = setpoint − measurement. A positive error means the output is below the target, so the controller must push it up; a negative error means it is above, so the controller must pull it down. Get this sign backwards and the controller pushes the robot away from the target: it runs away at full power instead of settling.

The plant: a motor as a first-order system

Robotics Fundamentals 3 gave the steady-state motor model, V = kω + IR and τ = kI (Basic Electronics 9). That model tells you where the speed ends up. To control the motor we also need to know how quickly it gets there, and the missing ingredient is the inertia J of everything the motor has to accelerate (rotor, gears and wheel, as seen from the motor shaft). Putting J dω/dt = kI − Tload together with the electrical equation (and ignoring the winding inductance, whose time constant is far shorter than the mechanical one for the small motors in this course) gives a standard result: the speed approaches its final value like a charging capacitor (compare the RC circuit of Basic Electronics 4).

tau(m) d(omega)/dt + omega = K ( u - d ) first-order motor model (u = applied volts, omega = shaft speed in rad/s) K = 1 / k the steady gain: rad/s for each volt (k = back-EMF constant of Robotics Fundamentals 3, V s/rad) tau(m) = J R / k^2 the mechanical time constant: how fast the speed responds (J = inertia in kg m^2, R = winding resistance in ohms) d = R T(load) / k the load, written as the number of volts it "eats" (T(load) = load torque in N m) Step response from rest at constant u: omega(t) = K ( u - d ) ( 1 - e^( -t / tau(m) ) ) final value K ( u - d ) Illustrative motor and wheel (a gearbox is included in k and J): k = 0.05 V s/rad, R = 2.0 ohm, J = 2.5e-4 kg m^2, load torque 0.025 N m K = 1 / 0.05 = 20 rad/s per volt tau(m) = 2.5e-4 x 2.0 / 0.05^2 = 5.0e-4 / 0.0025 = 0.20 s d = 2.0 x 0.025 / 0.05 = 1.0 V (the wheel needs 1 V of drive before it starts to speed up) With u = 6 V and the load: final speed = 20 x (6 - 1) = 100 rad/s; with no load it would be 20 x 6 = 120 rad/s Fraction of the final speed reached after n time constants: t / tau 1 2 3 4 5 fraction 63.2 % 86.5 % 95.0 % 98.2 % 99.3 % (1 - e^-n)
% of final speed 100 | ********************* 90 | *********** 80 | ***** 70 | *** 60 | *** 50 | ** 40 | * 30 | ** 20 | * 10 | * 0 |* +--------------------------------------------------- 0 1 2 3 4 5 time in units of tau(m) (one column = 0.1 tau)

Control engineers call this a first-order lag: a command that jumps instantly produces a speed that climbs smoothly, taking about 4τm (0.8 s here) to settle to within 2 %. For the simulations below we step the model forward in small, equal time steps dt, using the simplest method (Euler):

omega(next) = omega + ( dt / tau(m) ) x ( K ( u - d ) - omega ) with dt = 0.02 s the factor dt / tau(m) = 0.1 Check from rest, u = 6 V, d = 1 V: omega(1) = 0 + 0.1 x ( 20 x 5 - 0 ) = 10.0 rad/s omega(2) = 10 + 0.1 x ( 100 - 10 ) = 19.0 rad/s omega(3) = 19 + 0.1 x ( 100 - 19 ) = 27.1 rad/s (The exact curve reaches 9.5, 18.1, 25.9 rad/s: Euler is a slightly fast approximation. It is good enough to teach with, and it is exactly what a real program does at every tick.)

Open loop: why it is not enough

To hold 60 rad/s open loop, calculate the voltage from the model and send it: u = r / K + d = 60 / 20 + 1 = 4.0 V. That is a perfectly good controller on the day the model is right. The model is never right for long.

Open loop sends 4.0 V for a target of 60 rad/s. What actually happens: Model exactly right: 20 x ( 4.0 - 1.0 ) = 60 rad/s error 0 Battery sags so all voltages fall by 15 %: 20 x ( 0.85 x 4.0 - 1.0 ) = 48 rad/s error 12 rad/s (20 % low) The robot meets a carpet, load doubles (d = 2 V): 20 x ( 4.0 - 2.0 ) = 40 rad/s error 20 rad/s (33 % low) Closed loop (the controllers of the next sections) measures the speed, sees the error and raises the voltage until the error is small: the battery and the load no longer set the result.

Open loop is not useless: for a stepper motor driving a light load, or a quick rough move, it is simple, cheap and needs no sensor. A good controller usually combines both: an open-loop guess for the bulk of the command (feedforward, section 11) and a closed loop to clean up what the guess gets wrong.

2. Bang-bang control and hysteresis

The simplest closed-loop controller is a switch: if the speed is below the target, turn the motor fully on; otherwise turn it off. This is bang-bang (or on-off) control, and it is what a household thermostat does.

u = U(max) if e > 0 (measurement below the target) u = 0 otherwise Plain bang-bang cannot settle: the instant the speed crosses the target the output flips, the speed turns round and crosses back, and so on. The output switches as fast as the loop can run ("chatter"): hard on the motor driver, the contacts of a relay, and the gears, and it makes noise and heat.

The cure is hysteresis: two thresholds instead of one. Switch on only when the measurement falls below r − h, and off only when it rises above r + h; between the thresholds, keep doing whatever you were doing. The output now switches at a rate set by the plant, not by the loop speed, at the price of a ripple of ±h in the result.

Motor of section 1 (K = 20, tau(m) = 0.2 s, d = 1 V), target 60 rad/s, supply 6 V: On (u = 6 V): speed heads for 20 x (6 - 1) = 100 rad/s. Off (u = 0): speed heads for 20 x (0 - 1) = -20 rad/s (that -20 is only the limit of the formula; in the range of interest, near 60, the speed is falling toward it, which is all we use) Hysteresis h = 3 rad/s: on below 57, off above 63. Time to climb from 57 to 63 and to fall from 63 to 57 ( t = tau ln( (final - start) / (final - end) ) ): t(on) = 0.2 x ln( (100 - 57) / (100 - 63) ) = 0.2 x ln( 43 / 37 ) = 0.0301 s t(off) = 0.2 x ln( (63 + 20) / (57 + 20) ) = 0.2 x ln( 83 / 77 ) = 0.0150 s Period 0.0451 s, a switching rate of 22.2 Hz, and the speed ripples between 57 and 63. Hysteresis h = 6 rad/s: t(on) = 0.2 x ln( 46 / 34 ) = 0.0605 s, t(off) = 0.2 x ln( 86 / 74 ) = 0.0301 s, period 0.0905 s = 11.0 Hz, ripple 54 to 66. A wider band halves the switching rate and doubles the ripple: that is the whole trade.
rad/s (hysteresis +-3 around 60: on below 57, off above 63; one column = 0.003 s, 60 columns = 0.18 s) 63 | ** * ** * 62 | * * ** * * * ** * 61 | ** * * * ** * * * 60 |----**--------------**------*------**--------------**------* 59 | * * ** * * * ** 58 | ** * * * ** * * 57 |* ** * ** +------------------------------------------------------------

Bang-bang control is a good fit when the plant is slow and the result need not be precise: heaters, a battery charger, a fan, a pump filling a tank, or an obstacle avoider that simply turns on or off (Robotics Fundamentals 8). It is a poor fit for steering a wheel or arm smoothly, because the ripple and the shock of full power to full reverse are exactly what we do not want. For those the controller should give an output that is proportional to how far off the robot is.

3. Proportional control

A proportional (P) controller sets the output in proportion to the error: u = Kp e. A big error gets a big push, a small error a gentle one, and at zero error the output is zero. This is what makes it smooth: the push shrinks as the robot approaches the target, so there is no hard switching.

Use the motor model with a proportional gain Kp = 0.2 V per rad/s of error, a target of 60 rad/s, the 1 V load, a supply limit of 6 V, and a 20 ms step (dt = 0.02 s). At each step: e = 60 − ω, u = min(6, 0.2 e), then the Euler update of section 1.

step k time (s) omega (rad/s) error e Kp e u after the 6 V limit 0 0.00 0.00 60.00 12.00 6.00 (12 V wanted, only 6 V available: saturated) 1 0.02 10.00 50.00 10.00 6.00 2 0.04 19.00 41.00 8.20 6.00 3 0.06 27.10 32.90 6.58 6.00 4 0.08 34.39 25.61 5.12 5.12 (no longer saturated) 5 0.10 39.20 20.80 4.16 4.16 6 0.12 41.60 18.40 3.68 3.68 7 0.14 42.80 17.20 3.44 3.44 8 0.16 43.40 16.60 3.32 3.32 9 0.18 43.70 16.30 3.26 3.26 ... and it settles at 44.0 rad/s: 16 rad/s SHORT of the target Row check (k = 4): omega(5) = 34.39 + 0.1 x ( 20 x ( 5.12 - 1 ) - 34.39 ) = 34.39 + 0.1 x 48.05 = 39.20 rad/s (using u = 5.122 unrounded)

Steady-state error

The speed does not reach 60. It settles where the controller's output is just enough to hold the speed against the load, and with a purely proportional controller that output is only produced by an error. At rest in the steady state ω = K(u − d) and u = Kp e, with e = r − ω; solving gives the steady-state error:

omega = K ( Kp e - d ) and e = r - omega => e ( 1 + K Kp ) = r + K d => e(ss) = ( r + K d ) / ( 1 + K Kp ) Worked example (K = 20, d = 1 V, r = 60 rad/s, Kp = 0.2): e(ss) = ( 60 + 20 ) / ( 1 + 4 ) = 80 / 5 = 16 rad/s final speed 44.0 rad/s (27 % low: the table above) Without the load (d = 0): e(ss) = 60 / 5 = 12 rad/s final speed 48.0 rad/s Effect of the gain (same motor, load d = 1 V): Kp (V per rad/s) 0.1 0.2 0.5 K Kp (loop gain) 2 4 10 e(ss) = 80 / (1+K Kp) 26.67 16.00 7.27 rad/s final speed 33.33 44.00 52.73 rad/s The error shrinks like 1 / (1 + K Kp): a large gain helps, but never removes it. A bigger Kp also asks for a bigger voltage at the start (Kp r = 12 V for Kp = 0.2, 30 V for 0.5), which a 6 V supply cannot give, and, as sections 7 and 8 show, a larger Kp makes the loop ring and eventually go unstable. (Here Kp = 1 would predict 3.81 rad/s of error, but with a 20 ms update the loop is unstable above about Kp = 0.95, see section 8.)

A steady-state error is not a bug in the code; it is a property of proportional control with a load. A motor holding a heavy robot on a slope, a heater holding a room warm in winter, an arm holding up its own weight: each needs a non-zero output to stay where it is, and a P controller can only supply that by leaving an error. That is the job of the next term.

4. Integral control

The integral term adds up the error over time: I = Ki ∫ e dt, in discrete form I = I + Ki e dt at every step. While any error remains, the sum keeps growing and pushes the output harder, so the error cannot stay non-zero in the steady state: the integral stops changing only when e = 0. It has learned the load. A PI controller is u = Kp e + I.

Same motor, same target, now with Ki = 3 V per (rad/s · s) in addition to Kp = 0.2. The loop also uses the anti-windup rule explained below; the integral is not allowed to grow while the output is saturated.

k t (s) omega e Kp e I (held or updated) u wanted u sent (6 V limit) 0 0.00 0.00 60.00 12.00 0.000 15.600 6.00 saturated: integral NOT updated 1 0.02 10.00 50.00 10.00 0.000 13.000 6.00 2 0.04 19.00 41.00 8.20 0.000 10.660 6.00 3 0.06 27.10 32.90 6.58 0.000 8.554 6.00 4 0.08 34.39 25.61 5.12 0.000 6.659 6.00 (would be 1.537 if updated; still saturated) 5 0.10 40.95 19.05 3.81 1.143 4.953 4.95 first step the output is inside the limit 6 0.12 44.76 15.24 3.05 2.057 5.105 5.10 7 0.14 48.50 11.50 2.30 2.748 5.049 5.05 8 0.16 51.74 8.26 1.65 3.243 4.894 4.89 9 0.18 54.36 5.64 1.13 3.582 4.710 4.71 10 0.20 56.34 3.66 0.73 3.801 4.533 4.53 11 0.22 57.77 2.23 0.45 3.935 4.380 4.38 ... settling toward 60.00 Row check (k = 5): I = 0 + 3 x 19.05 x 0.02 = 1.143; u = 3.81 + 1.143 = 4.953 V. Final state: e = 0, omega = 60.00 and I = 4.0 V (the integral has become the whole command: u = r / K + d = 60 / 20 + 1 = 4.0 V). The PI result: peak 60.16 rad/s (0.3 % overshoot), within 2 % of the target from about 0.26 s, and NO steady-state error. P alone left 16 rad/s.
rad/s o = P only (Kp = 0.2) x = PI (Kp = 0.2, Ki = 3, with anti-windup) target 60 (dashes); one column = 0.02 s, 31 columns = 0.6 s 70 | 65 | 60 |-----------xxxxxxxxxxxxxxxxxxxx 55 | xx 50 | xx 45 | xoooooooooooooooooooooooo 40 | xo 35 | x 30 | 25 | x 20 | x 15 | 10 | x 5 | 0 |x +-------------------------------

Integral windup and anti-windup clamping

The integral term has a weakness. Suppose the output is already at its limit (6 V here), so the robot cannot go any faster, yet the error is large for a long time (a big step, a blocked wheel, a hill too steep). A naive integrator keeps adding up the error even though the extra output cannot be delivered. The integral winds up to a huge value, like a spring being wound far beyond what the mechanism can use. When the speed finally reaches the target, the error drops to zero, but the integral is still enormous, and the controller keeps driving at full power until the error has been negative for long enough to unwind it. The result is a big overshoot. Here is the same loop without protection:

Without anti-windup (the integral always adds): k t (s) omega e Kp e I u wanted u sent 0 0.00 0.00 60.00 12.00 3.600 15.600 6.00 1 0.02 10.00 50.00 10.00 6.600 16.600 6.00 2 0.04 19.00 41.00 8.20 9.060 17.260 6.00 4 0.08 34.39 25.61 5.12 12.571 17.693 6.00 (the integral is already 12.6 V: 3 times what the loop will ever need) 8 0.16 56.95 3.05 0.61 15.155 15.764 6.00 integral peaks at 15.15 V 9 0.18 61.26 -1.26 -0.25 15.079 14.828 6.00 the target is passed, but the output is STILL saturated at 6 V 11 0.22 68.62 -8.62 -1.72 14.254 12.530 6.00 and the speed keeps climbing Result: the speed peaks at 81.47 rad/s, an overshoot of 35.8 % of the target, against 0.3 % with anti-windup; it takes about 0.5 s to settle against 0.26 s.

The cure is anti-windup. The simplest and most common form is clamping (also called conditional integration): do not let the integral grow when the output is saturated and the error would push it further into saturation. That is exactly what the PI table above did in rows 0 to 4. Two other common forms: clamp the integral itself to a sensible range (for example, never more than the supply voltage), or back-calculate (feed the amount by which the output was clipped back into the integral so it unwinds). Any robot with output limits (which is every robot) needs one of these. Windup also appears whenever the plant is stuck: a robot pressing against a wall while the position controller keeps asking for more.

5. Derivative control

The derivative term responds to the rate of change of the error: Kd de/dt. In discrete form, Kd (e − eprevious) / dt. It reacts to where the error is heading: if the error is shrinking quickly (the robot is rushing toward the target) the derivative is negative and subtracts from the output, braking early, so the robot arrives gently instead of overshooting. It acts like damping: a shock absorber, or a hand that steadies a swinging pendulum. Section 7 quantifies this. It does nothing about a steady error (the derivative of a constant is zero), so it is a partner to P, not a replacement.

Two problems: noise and kick

Differentiating a signal amplifies everything fast in it, including noise. A measurement that jumps by one count of the encoder or one step of the ADC produces a large derivative. And if the setpoint is changed suddenly, the error steps with it, and the derivative of a step is, in principle, infinite.

Position loop of section 7 (Kd = 0.0914 V per rad/s), sample time dt = 0.01 s, output limit 6 V. Noise: the encoder reading changes by one count, 1 degree = 0.01745 rad, in a single sample: rate seen = 0.01745 / 0.01 = 1.745 rad/s derivative output = 0.0914 x 1.745 = 0.16 V every time a count changes, although the real speed did not. Halve dt and the same count step gives twice the rate and twice the spike: differentiation gets worse as the loop gets faster. Kick: the target is changed by 1 rad in one sample (the error steps from 0 to 1 rad): derivative on the ERROR = Kd x ( 1 - 0 ) / 0.01 = 0.0914 x 100 = 9.14 V for one sample: more than the whole 6 V supply, a violent jerk.

Two standard fixes. Derivative on measurement: since the setpoint r does not change between steps, de/dt = dr/dt − dy/dt reduces to −dy/dt except at the moment the setpoint moves. Compute the derivative term as −Kd (y − yprevious) / dt and the kick disappears completely (the measurement cannot jump when the target changes). And filter the derivative with a first-order filter (Robotics Fundamentals 5) so noise is smoothed before it is multiplied by Kd; commonly the filter time constant is set at a small fraction of Kd/Kp. Better still, use a sensor that measures the rate directly: a gyro gives the turn rate, and an encoder with a good speed estimate gives velocity, so the “derivative” of a heading or position is measured, not differentiated (section 12).

6. The PID controller in discrete time

Adding the three terms gives the PID controller. In continuous form, and in the form the robot's program computes at every tick:

Continuous: u(t) = Kp e(t) + Ki integral( e dt ) + Kd de/dt Discrete (every dt seconds): e = r - y P = Kp e I = I + Ki e dt (the running sum, which is the controller's memory) D = -Kd ( y - y(previous) ) / dt (derivative on measurement) u = P + I + D then limit u to its allowed range, and keep I from winding up (section 4) Alternative form: u = Kp ( e + (1/Ti) integral(e dt) + Td de/dt ) with Ki = Kp / Ti, Kd = Kp Td (Ti = integral time, Td = derivative time, both in seconds) Ti is roughly "the time over which the integral repeats the proportional correction"; Td is roughly "how far ahead the derivative is looking". The two forms are the same controller; the tuning rules in section 9 use Ti and Td. What each term does (an increase of each gain): gain rise time overshoot settling time steady-state error stability Kp shorter larger little change smaller (not zero) worse Ki a little shorter larger longer removed worse Kd little change smaller shorter no effect better (while noise is controlled) (typical trends for a simple plant, not laws for every system)

Choosing dt, and limiting the output

  • The loop must be much faster than the plant. A common rule is at least 10 updates per time constant of the closed loop you want. For the 0.2 s motor, 20 ms is the largest step worth using, and 10 ms is better; a small robot's speed loop typically runs between 50 Hz and 1 kHz (a rule of thumb, approximate). Euler stepping of the plant, as in this chapter, also needs dt well below τm.
  • But not faster than the measurement. A speed measured from encoder counts has a quantisation error that grows as the interval shrinks (Robotics Fundamentals 6 section 2: 2.6 % at 20 ms, 0.5 % at 100 ms for the 0.3 m/s example). The derivative term makes it worse (previous section). There is no use running the loop faster than new information arrives.
  • Use the real dt. If the loop time varies (jitter), compute dt from a timer every tick, as with the gyro of Robotics Fundamentals 6; a program that assumes 20 ms but runs at 21 ms makes Ki and Kd wrong by 5 %.
  • Always limit the output to what the hardware can deliver (the supply voltage, or ±100 % duty cycle), and tie the integral to that limit (anti-windup). For a motor, the command is converted to a PWM duty cycle: duty = u / Vsupply. Real motors also have a deadband (a minimum voltage below which they do not turn because of friction) that is sometimes handled by adding a small constant offset; the integral term also takes care of some of it.

The PID as pseudocode, and a plant to test it on

Hardware-neutral Python-style pseudocode of the controller of the previous section, with derivative on measurement and clamping anti-windup, and a three-line simulation of the first-order motor so that the controller can be tried on a laptop before it ever sees a motor:

class PID: def __init__(self, kp, ki, kd, out_min, out_max): self.kp, self.ki, self.kd = kp, ki, kd self.out_min, self.out_max = out_min, out_max self.integral = 0.0 self.prev_meas = None def update(self, setpoint, measurement, dt): error = setpoint - measurement p = self.kp * error if self.prev_meas is None: d = 0.0 # no history on the first call else: d = -self.kd * (measurement - self.prev_meas) / dt # derivative on measurement self.prev_meas = measurement new_integral = self.integral + self.ki * error * dt u = p + new_integral + d u_limited = min(self.out_max, max(self.out_min, u)) # clamping: do not integrate if saturated AND the error pushes further into saturation if u == u_limited or (u - u_limited) * error < 0: self.integral = new_integral return u_limited def motor_step(omega, u, load_volts, dt, K=20.0, tau=0.2): return omega + (dt / tau) * (K * (u - load_volts) - omega) # the Euler step of section 1 pid = PID(0.2, 3.0, 0.0, -6.0, 6.0) omega = 0.0 for k in range(150): # 3 seconds at dt = 0.02 u = pid.update(60.0, omega, 0.02) omega = motor_step(omega, u, 1.0, 0.02)

Running this reproduces the PI table of section 4 row by row, which is the best test of a PID class: before a robot ever uses it, check it against a plant whose answer you can calculate by hand.

7. Position control: second-order behaviour, overshoot and oscillation

So far the quantity controlled was speed. Controlling a position (the angle of an arm joint, the distance driven) is harder in an important way: the motor integrates speed into position. Even with no integral term in the controller, the plant itself contains one. The motor of section 1 becomes θ′ = ω, τm ω′ + ω = K u (no load, for clarity), which is a second-order system, and a second-order system can ring.

Close the loop with a P controller, u = Kp (r − θ). Substituting gives the standard form of a second-order system, a mass on a spring with some damping:

theta'' + (1 / tau(m)) theta' + ( K Kp / tau(m) ) theta = ( K Kp / tau(m) ) r compare the standard form x'' + 2 zeta wn x' + wn^2 x = wn^2 r Natural frequency: wn = sqrt( K Kp / tau(m) ) (rad/s: how fast it would ring if nothing damped it; frequency = wn / 2 pi hertz) Damping ratio: zeta = 1 / ( 2 sqrt( K Kp tau(m) ) ) (a pure number: how strongly the ringing is damped) In plain terms: zeta = 0 : never dies down, rings for ever zeta = 0.2 to 0.5 : rings several times, large overshoot zeta = 0.7 : about the best compromise, a small overshoot, then done zeta = 1 : critically damped, the fastest response with NO overshoot zeta > 1 : overdamped, slow and creeping, no overshoot at all For zeta below 1 (a step of size 1 in the target): Overshoot (fraction of the step): OS = exp( - pi zeta / sqrt( 1 - zeta^2 ) ) Time of the first peak: t(peak) = pi / ( wn sqrt( 1 - zeta^2 ) ) Period of the ringing: T(d) = 2 pi / ( wn sqrt( 1 - zeta^2 ) ) Time to settle within 2 %: about 4 / ( zeta wn ) (an approximation) Overshoot against damping: zeta 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 OS % 52.7 37.2 25.4 16.3 9.5 4.6 1.5 0.2

Worked example with the motor of section 1 (K = 20 rad/s per volt, τm = 0.2 s), a 1 rad step, and no load:

Controller Kp (V/rad) Kd (V per rad/s) wn (rad/s) zeta overshoot first peak settles (2 %) P only 1.0 0 10.00 0.250 44.4 % 0.32 s about 1.6 s rings, many swings P only 0.5 0 7.07 0.354 30.5 % 0.47 s about 1.6 s P only 0.125 0 3.54 0.707 4.3 % 1.26 s about 1.6 s small overshoot, but slow P only 0.0625 0 2.50 1.000 none - about 2.3 s critically damped (simulated) PD 1.0 0.0914 10.00 0.707 4.3 % 0.45 s about 0.57 s same fast wn, but well damped The first row: wn = sqrt( 20 x 1.0 / 0.2 ) = sqrt( 100 ) = 10.0 rad/s, zeta = 1 / ( 2 sqrt( 20 x 1.0 x 0.2 ) ) = 1 / ( 2 x 2 ) = 0.25, OS = exp( -pi x 0.25 / sqrt( 1 - 0.0625 ) ) = exp( -0.811 ) = 44.4 % first peak pi / ( 10 x 0.968 ) = 0.32 s; one full swing 2 pi / 9.68 = 0.65 s (a wobble at 1.5 Hz). The PD row: 2 zeta wn = ( 1 + K Kd ) / tau(m) = ( 1 + 20 x 0.0914 ) / 0.2 = 14.14, so zeta = 14.14 / ( 2 x 10 ) = 0.707. (Kd = 0.0914 was chosen to give exactly that.) (A step-by-step simulation of these loops with a small time step agrees with the formulas to within about 1 % of the step in overshoot.)
position (target = 1.0, dashes) a = P, Kp = 1.0 (zeta 0.25) b = P, Kp = 0.125 (zeta 0.707) c = PD, Kp = 1.0, Kd = 0.0914 (zeta 0.707) one column = 0.04 s, 51 columns = 2.0 s 1.5 | a 1.4 | a a 1.3 | a a 1.2 | a 1.1 | a a aaaaaa 1.0 |--------ccccccccccccccccccccccccccccccccccccccccccc 0.9 | c a bbb 0.8 | a c bbba 0.7 | c bb 0.6 | c bb 0.5 | a b 0.4 | c bb 0.3 | a b 0.2 | c bb 0.1 | c bb 0.0 |cbb +---------------------------------------------------

Why robots oscillate

Read the damping formula again: ζ = 1 / (2√(K Kp τm)). As the gain Kp rises, the damping falls. The loop gets a stronger spring and the same weak damping, so it rings. This is the single most important fact about tuning and it explains most wobbling robots:

  • Gain too high. The controller pushes harder than the plant's own damping can absorb. Overshoot grows, then the swings take longer to die, and finally they do not die at all.
  • Not enough damping. Friction alone is weak damping, which is why a position loop with only P is easy to make ring. The remedy is the derivative term (the PD row above): it supplies the damping that the plant lacks, so a high gain (fast response) can be kept without the ringing.
  • Delay. A late measurement means the controller reacts to where the robot was, so its correction arrives out of step with the motion (next section).
  • Backlash and friction. The loose fit in a gearbox (Robotics Fundamentals 3) is a dead zone in which the motor moves but the output does not, so the loop sees no response and pushes harder, then overshoots when the gears engage. An integral term plus static friction (stiction) causes hunting: the integral slowly builds up until the joint breaks free, overshoots, and the cycle begins again.
  • Integral too large (section 4): it overshoots by design and, when large, produces a slow oscillation of its own.

8. Stability, delay and gain, in plain words

A control loop is stable if any disturbance dies away, and unstable if it grows. Think of steering a boat by watching its wake: if you are slow to react, you correct for a course error that is already gone, and your correction makes the next error larger. Every real loop has some delay (time between something happening and the controller acting on it): the sensor filter of Robotics Fundamentals 5, the interval over which speed is measured (Robotics Fundamentals 6), the time to compute, a communications link, the motor's own lag. A delay turns a correction that should oppose the error into one that arrives a half-swing late and adds to it. At a frequency where the delay shifts the correction by half a cycle, the loop is stable only if the correction is smaller than the error it was trying to fix; above a certain gain, called the ultimate gain Ku, each swing is bigger than the last.

Take the speed loop of section 1 and add a delay of three samples (60 ms: sensor filtering and command latency, a plausible value for a small robot, approximate). With P control and a 60 rad/s target the loop behaves as follows (each row is a simulation, 20 ms step, no load, no output limit):

Kp (V per rad/s) loop gain K Kp final speed (rad/s) first swing above final next swing / this swing behaviour 0.05 1.0 30.00 0.50 -0.02 smooth, almost no overshoot 0.10 2.0 40.00 10.05 -0.23 overshoots, dies out in 2 or 3 swings 0.15 3.0 45.00 24.22 -0.50 rings: each swing half the last 0.20 4.0 48.00 40.68 -0.76 rings for a long time 0.2427 4.86 49.75 55.38 -1.00 sustained oscillation: the ultimate gain Ku 0.30 6.0 51.43 74.40 -1.31 each swing 31 % larger: UNSTABLE (the final value is K Kp r / ( 1 + K Kp ), the same as without the delay; the minus sign means the next swing is on the other side of the final value; Ku = 0.243 and the period of the sustained oscillation, Pu = 0.25 s, come from the simulation of exactly this loop.) The same limit exists WITHOUT any extra delay: the Euler update is itself a one-step delay. With no delay the loop is stable only while ( dt / tau(m) ) ( 1 + K Kp ) < 2, i.e. Kp < ( 2 tau(m) / dt - 1 ) / K = ( 20 - 1 ) / 20 = 0.95 for dt = 0.02 s (Kp = 0.9 decays, alternating, and Kp = 1.0 grows, in the simulation). Rule of thumb: delay sets the speed limit. The more delay in a loop (a slow sensor, a slow filter, a slow loop), the lower the gain it can use, and the slower the response it can safely have.

The practical lessons: use the fastest, least-delayed measurement you can (a gyro rate rather than a heading difference, an encoder rate with a short interval); do not filter harder than you must (Robotics Fundamentals 5), because a filter is a delay; run the loop at a steady, fast rate; and stay well below the ultimate gain. A loop at half of Ku is not “half as stable”; it is comfortably stable, with a moderate overshoot, and has margin for the motor, battery and floor to change.

9. Tuning

Tuning means choosing Kp, Ki and Kd so the response is fast without being unstable. There is a model-based way (calculate them from the plant, as in sections 3 and 7 where ζ and ωn followed from K and τm) and several experimental ways, because the real plant is never as clean as the model.

Manual tuning, in order

  1. Make it safe first. Wheels off the ground or a slow, limited speed, an output limit set low, and a stop switch to hand (Robotics Fundamentals 10 covers safe testing). A badly tuned loop can swing a heavy arm or run a robot off a table.
  2. Set Ki = Kd = 0. Raise Kp in steps, applying a step in the target each time, until the response is fast and starts to overshoot or ring, then back off to somewhere between a half and two thirds of that value.
  3. Add Kd (for position or heading loops, where it helps most) until the ringing is controlled and the overshoot is small. Stop when the output starts to look noisy or buzzes. If the loop is a plain speed loop, with a lag but no ringing, Kd is often not needed at all.
  4. Add Ki, small at first, until the steady-state error is removed in a time that suits the task; too much gives overshoot and slow oscillation. Test with the load changed, and with a big step to see windup.
  5. Re-test everything at the extremes: low battery, high load, full speed. Choose gains that are good enough everywhere, not best at one operating point.

The Ziegler-Nichols ultimate gain method

The classic experimental recipe (Ziegler and Nichols, 1942) needs no model. Using only a P controller, raise Kp until the loop produces a steady, sustained oscillation (neither growing nor dying). The gain at that point is the ultimate gain Ku; the period of the oscillation is the ultimate period Pu. The standard table converts the two numbers into controller settings (quoted from memory of the original table; check it before relying on it):

Controller Kp Ti Td (Ki = Kp / Ti, Kd = Kp Td) P 0.5 Ku - - PI 0.45 Ku Pu / 1.2 - PID 0.6 Ku Pu / 2 Pu / 8 Worked example on the delayed speed loop of section 8: Ku = 0.24 (simulation: 0.2427), Pu = 0.25 s (simulation: 0.248). P: Kp = 0.5 x 0.24 = 0.12 PI: Kp = 0.45 x 0.24 = 0.108, Ti = 0.25 / 1.2 = 0.208 s, Ki = 0.108 / 0.208 = 0.5184 PID: Kp = 0.6 x 0.24 = 0.144, Ti = 0.25 / 2 = 0.125 s, Td = 0.25 / 8 = 0.03125 s, Ki = 0.144 / 0.125 = 1.152, Kd = 0.144 x 0.03125 = 0.0045 Step response of the delayed motor loop (target 60 rad/s, no load, 20 ms step, 60 ms delay, derivative on measurement), by simulation: Controller overshoot settles within 2 % note Ziegler-Nichols P (Kp 0.12) - - rings and ends at 42.35 rad/s (the P offset: 17.65 short) Ziegler-Nichols PI (0.108, 0.5184) 27.9 % 0.60 s Ziegler-Nichols PID (0.144, 1.152, 0.0045) 53.6 % 0.72 s peak 92.2 rad/s for a 60 rad/s target Hand-tuned PI (Kp 0.1, Ki 0.5) 23.8 % 0.50 s found by raising Ki until the overshoot became uncomfortable Hand-tuned PI (Kp 0.1, Ki 1.0) 63.8 % 1.08 s too much integral: big overshoot, slow settling "Some overshoot" variant (Kp Ku/3, Ti Pu/2, Td Pu/3): 21.5 % 1.12 s Kp 0.08, Ki 0.64, Kd 0.0067 "No overshoot" variant (Kp 0.2 Ku, Ti Pu/2, Td Pu/3): 14.0 % 0.96 s Kp 0.048, Ki 0.384, Kd 0.004 (a name that promises more than it gives here)

What Ziegler-Nichols gets wrong

  • It is aggressive. The table aims for a response in which each swing is about a quarter the size of the last. That is fast but rings, as the 54 % overshoot above shows; most robots want something gentler. Treat the result as a first guess, then reduce Kp and Ki until the behaviour is acceptable. The variants of the table that claim “some” or “no” overshoot are, again from memory, less aggressive but still overshoot here.
  • It requires driving the system to the edge of instability. That is awkward with a delicate or heavy mechanism: a robot arm swinging at its ultimate gain can slam into its stops, and in a saturated actuator the “sustained oscillation” is clipped and ill-defined. Measure with low output limits and a stop switch to hand.
  • The measurements are fuzzy. Finding the exact gain of a steady oscillation is hard on a real robot with noise, backlash and friction, and a 10 % error in Ku or Pu carries straight into the gains.
  • It assumes a plant with a delay and a lag. A pure position loop (a plant with an integrator, section 7) has no ultimate gain at all without delay, only the delay of the sampling and sensor, and the table was built for process-control plants rather than motors.
  • It optimises the wrong thing for robots. It was tuned for rejecting disturbances, not for following a changing setpoint smoothly, which is what most robot motion needs.
  • Other methods exist: relay auto-tuning (the controller switches between two outputs and measures the resulting oscillation, which is gentler than driving the loop unstable), the “reaction curve” method (measure the step response of the plant and fit a delay and a time constant), and model-based design as in sections 3 and 7. Many motor-control libraries include an auto-tune routine of the first kind.

10. Cascaded loops

A position controller does not have to command the motor voltage directly. A common and effective structure is a cascade: an inner loop controls speed (fast, at 50 to 1000 Hz, using the encoder rate), and an outer loop controls position (slower), where the outer controller's output is not a voltage but a speed setpoint for the inner loop.

position --->( + )--- e(pos) --> [ P: Kpos ] -- speed target -->( + )-- e(speed) --> [ PI ] -- u --> [ motor ] --+--> speed ---> [ integrate ] --+--> position target ^ - ^ - | | | +------------- encoder speed -------------+ | +-------------------------------------------------------- encoder position ---------------------------------------------+ Advantages: the inner loop makes the motor behave like a simple, well-damped speed source whatever the load; the speed limit is just a clamp on the speed target; the outer loop is then easy to tune (a plain P controller gives smooth approach with no overshoot); the damping needed to avoid ringing comes from the inner loop. Rule: the outer loop must be clearly SLOWER than the inner loop (a factor of three to ten is typical): the inner loop has to follow the speed target faster than it changes.

Using the inner PI controller of section 4 (Kp = 0.2, Ki = 3, 6 V limit, 1 V load, which settles within 2 % in about 0.26 s) and an outer proportional gain Kpos in rad/s of speed target per radian of error, a 2 rad step of position gives:

Kpos (1/s) speed target at the start overshoot settles within 2 % (0.04 rad) 4 / Kpos (ideal inner loop) 3 6 rad/s 0.0 % 1.30 s 1.33 s 5 10 rad/s 0.0 % 0.78 s 0.80 s 8 16 rad/s 0.0 % 0.50 s 0.50 s 15 30 rad/s 7.8 % 0.48 s 0.27 s the outer loop is now faster than the inner can follow With an ideal inner loop the outer loop is a first-order lag with a time constant of 1 / Kpos, so it takes 4 / Kpos to settle: the rows above agree up to Kpos = 8. At 15 the outer loop asks for changes faster than the speed loop can deliver, and the extra lag appears as overshoot. A speed limit is simply min( wmax, Kpos x error ).

11. Feedforward

A feedback controller reacts only after an error has appeared. If you already know roughly what output a target needs, add it directly and let the feedback correct only the remainder. This is feedforward: u = uff + uPID. For the motor, the model gives the first guess uff = r / K (plus an estimate of the load, if known); for a moving target it also includes terms for speed and acceleration. In an arm, the feedforward often carries the weight of the arm against gravity (Robotics Fundamentals 9).

Motor of section 1, target 60 rad/s, true load d = 1 V (unknown to the controller), Kp = 0.2. P only: final speed 44.0 rad/s, error 16 Feedforward u(ff) = r / K = 60 / 20 = 3.0 V plus the same P: omega = ( K ( u(ff) - d ) + K Kp r ) / ( 1 + K Kp ) = ( 20 x 2 + 20 x 0.2 x 60 ) / 5 = ( 40 + 240 ) / 5 = 56.0 rad/s, error 4 (a quarter of the error of the P controller) Feedforward plus PI: the integral only has to supply the missing 1 V instead of the whole 4 V, so it makes the same final result with much less work (in simulation: peak 60.26 rad/s, within 2 % at 0.22 s against 0.26 s for the PI alone: for this large first step, which saturates the output, the gain is small). Feedforward cannot become unstable, because it contains no feedback; and it removes most of the work from the feedback loop. Its weakness is the model: if K or d is wrong the output is wrong, which is exactly what the feedback is for.

12. PID examples: line following and heading hold

Line follower

The line sensor array of Robotics Fundamentals 5 produces a single number x: the position of the line in millimetres, positive to the right. The target is x = 0 (line under the centre), so the error is e = 0 − x = −x. The controller's output u is a turn rate in rad/s, counter-clockwise positive (the convention of Robotics Fundamentals 4), and it is turned into wheel speeds with the differential-drive relation vR = v + uL/2, vL = v − uL/2. A line to the right gives x > 0, so e < 0, so u < 0: a clockwise turn, toward the line.

Robot: base speed v = 0.20 m/s, track width L = 0.15 m, loop time dt = 0.01 s, line position x = +4.69 mm (the reading of Robotics Fundamentals 5) after +4.19 mm one step ago. Kp = 0.08 rad/s per mm, Kd = 0.002 rad/s per (mm/s) e = -4.69 mm P = 0.08 x ( -4.69 ) = -0.375 rad/s de/dt = ( -4.69 - ( -4.19 ) ) / 0.01 = -50 mm/s D = 0.002 x ( -50 ) = -0.100 rad/s (the line is moving away to the right: steer a little harder) Proportional term alone: u = -0.375 rad/s: vR = 0.200 + ( -0.375 ) x 0.075 = 0.172 m/s, vL = 0.200 - ( -0.375 ) x 0.075 = 0.228 m/s (the right wheel slower: turn right) With the derivative: u = -0.475 rad/s: vR = 0.164 m/s, vL = 0.236 m/s The line follower is a position-style loop: the controller steers (changes heading), and the heading integrates into the lateral position of the robot. So it is second-order and RINGS, exactly as in section 7: too much Kp makes the robot snake from side to side; the derivative is the standard cure. The integral is rarely used (a steady line offset on a straight line is small). A sensor mounted far ahead of the wheels adds delay between steering and seeing the result (section 8), a second reason to keep Kp modest, and to slow down on bends: speed v = v(max) / (1 + c |e|) is common.

Heading hold

To drive straight (or turn to a bearing) the target is a heading and the measurement is the gyro-based heading of Robotics Fundamentals 6. The error must be wrapped (section 9 of that chapter), or a target of −170 degrees and a heading of +175 degrees would give a 345 degree error and a robot that spins the long way round. The controller's derivative can come straight from the gyro: the heading's rate of change is the gyro rate ωgyro, so the derivative term is −Kd ωgyro, with no differentiation of a noisy angle at all.

e = wrap( target - heading ) u = Kp e - Kd x gyro_rate (u = turn rate command, rad/s, counter-clockwise positive) Worked example: target = -170 degrees, heading = +175 degrees, gyro says +10 degrees/s (already turning the right way). e = wrap( -170 - 175 ) = wrap( -345 ) = +15 degrees = 0.2618 rad (turn counter-clockwise by 15 degrees, not clockwise by 345) Kp = 3.0 per second, Kd = 0.4: P = 3.0 x 0.2618 = 0.785 rad/s D = -0.4 x ( 10 degrees/s = 0.1745 rad/s ) = -0.070 rad/s u = 0.716 rad/s The gyro rate acts as damping: the faster the robot is already turning, the more the command is reduced, so it eases onto the heading instead of swinging past it. Because the plant here is the robot's turning, which has inertia and the lag of the wheel speed loops (inner loops, section 10), heading hold is a cascade: the heading loop sets wheel speed differences for the speed loops.

13. Challenges

Challenge 1: The motor model, open loop, bang-bang and proportional control
A motor with its wheel has k = 0.04 V s/rad, R = 1.5 ohm and J = 2.4e-4 kg m². (a) Find K and τm, and the time to reach 63.2 %, 95 % and 98.2 % of the final speed. (b) The wheel load is equivalent to d = 1.2 V; find the load torque. Open loop, find the voltage that gives 75 rad/s. What speed results if the battery sags so that every voltage is 10 % lower, and what is the error as a percentage of the target? What if the load rises to d = 2.0 V instead? (c) A P controller with Kp = 0.16 V per rad/s and the 1.2 V load, target 75 rad/s: find the steady-state error and the final speed, then the same with no load. What gain Kp would reduce the load error to 3 rad/s, and why is it not practical on a 6 V supply? (d) Bang-bang control with a 6 V supply and a target of 75 rad/s, hysteresis ±4 rad/s: find the speed the motor would head for with the supply on and with it off, the time to climb and to fall across the band, the switching frequency, and the frequency with ±8 rad/s instead.
View solution
Challenge 2: PI control, windup, position loop, damping and derivative problems
(a) Simulate the speed loop K = 20, τm = 0.2 s, dt = 0.02 s, load d = 0.5 V, output limit ±5 V, target 40 rad/s, Kp = 0.1, Ki = 2.0, starting from rest. Tabulate steps 0 to 5 (ω, e, Kp e, I, the wanted u, and the u sent) twice: once with the integral always adding, once with clamping anti-windup (do not integrate if saturated and the error pushes further). Compare the integral values at step 5. What will the integral equal when the loop has settled? (b) A position loop with the same K and τm, Kp = 0.8 V/rad. Find ζ, ωn and the overshoot with Kd = 0. Then add Kd = 0.06 V per rad/s: find ζ, ωn, the overshoot, the time of the first peak, the period of the ringing and the approximate 2 % settling time. (c) At dt = 0.01 s with Kd = 0.06, the target changes by 0.5 rad in one step and the encoder has a resolution of 0.0105 rad. Find the derivative output when taken on the error at the instant of the change, and the noise it takes from a one-count change in the measurement; what is the first value when the derivative is taken on the measurement instead?
View solution
Challenge 3: Tuning, stability, feedforward and PID in a robot
(a) A speed loop with P control oscillates steadily at Ku = 0.35 with a period Pu = 0.18 s. Give the Ziegler-Nichols settings (Kp, Ti, Td, and Ki and Kd) for the P, PI and PID controllers. (b) Find the overshoot for ζ = 0.5, and the damping ratio that gives exactly 10 % overshoot. (c) A loop with K = 20 and τm = 0.2 s is updated every dt = 0.05 s by the Euler rule with no extra delay. Find the largest Kp for which it is stable, and the value if dt is cut to 0.02 s. (d) A motor with K = 25 has a true load of d = 1.2 V and a P gain of 0.16; the target is 75 rad/s. Find the final speed and error for P alone, and with feedforward uff = r / K (no load assumed). (e) A line follower: v = 0.25 m/s, L = 0.16 m, dt = 0.01 s, x = −6.5 mm now and −5.9 mm one step ago, Kp = 0.06 rad/s per mm, Kd = 0.0015 rad/s per (mm/s). Find e, de/dt, u and both wheel speeds, and say which way the robot turns. (f) Heading hold: target −170 degrees, heading +175 degrees, gyro +10 degrees/s, Kp = 3, Kd = 0.4. Find the wrapped error and the turn rate command u, and say which way the robot turns.
View solution

Quick reference

  • Error = setpoint − measurement. Open loop trusts the model; closed loop measures and corrects. Open loop at 4.0 V gives 60 rad/s only while the battery and load are as modelled.
  • Motor model: τm ω′ + ω = K (u − d); K = 1/k; τm = J R / k2; d = R T / k; step response K (u − d)(1 − e−t/τ): 63.2 %, 86.5 %, 95.0 %, 98.2 % after 1, 2, 3, 4 τ. Euler: ω += (dt/τ)(K (u − d) − ω).
  • Bang-bang: full on or off; chatters. Hysteresis ±h gives a ripple of ±h at a rate set by the plant: t = τ ln((final − start)/(final − end)).
  • P: u = Kp e; leaves ess = (r + K d)/(1 + K Kp) with a load; larger Kp shrinks it but never removes it, and makes the loop ring.
  • I: I += Ki e dt; removes the steady-state error (it learns the load: uss = r/K + d). Windup: saturation plus integrating gives a big overshoot (35.8 % against 0.3 % in the example); fix by clamping the integral when saturated.
  • D: −Kd (y − yprev)/dt (on measurement, to avoid kick); adds damping, amplifies noise, so filter it or use a gyro or speed measurement directly.
  • Discrete PID: u = Kp e + I + D, limited to the output range; Ki = Kp/Ti, Kd = Kp Td; choose dt at least 10 per closed-loop time constant, but no faster than the measurement; use the real dt.
  • Second order (position loop): ωn = √(K Kp / τm); ζ = 1 / (2 √(K Kp τm)); overshoot = exp(−πζ/√(1 − ζ2)); tpeak = π / (ωn √(1 − ζ2)); settling about 4 / (ζ ωn); ζ = 0.7 gives 4.6 %. With PD: 2 ζ ωn = (1 + K Kd)/τm. Higher Kp means lower ζ: robots oscillate when gain is high and damping low.
  • Stability: delay (sensor, filter, computation, loop rate) sets an ultimate gain Ku above which each swing grows; stay well below it. Without any extra delay, Euler at dt needs Kp < (2τm/dt − 1)/K.
  • Tuning: Kp up to just under ringing, then Kd for damping, then a little Ki, then test at the extremes. Ziegler-Nichols: P 0.5 Ku; PI 0.45 Ku, Pu/1.2; PID 0.6 Ku, Pu/2, Pu/8; fast but aggressive (about 54 % overshoot here), needs a near-unstable test, a first guess only.
  • Cascade: fast inner speed loop, slower outer position loop whose output is a speed target; the outer loop must be clearly slower. Feedforward: u = uff + PID, with uff = r / K; it does most of the work and the feedback trims it.
  • Line follower: e = −x, output a turn rate u, vR = v + uL/2, vL = v − uL/2. Heading hold: e = wrap(target − heading), u = Kp e − Kd × gyro rate.

Next, Robotics Fundamentals 8 steps up from how a robot moves to what it decides to do. Behaviour and decision-making: reactive behaviours, state machines, line following, obstacle avoidance and wall following built on the controllers of this chapter, and a first look at simple planning, mapping and localisation (using the landmarks and the frames of Robotics Fundamentals 6) so that a robot can choose where to go as well as get there.