Prerequisite: 03-pid-controller.md (continuous PID, tuning)
Unlocks: 05-mcu-motor-control.md (hardware that executes the discrete PID)
The PID equations in Lesson 03 are continuous-time — they assume infinitely fast computation with infinitely precise math. Real controllers run on a microcontroller that:
The gap between continuous theory and discrete practice causes real bugs: - Aliasing: Sample too slowly and fast disturbances appear as slow ones - Quantization: Encoder resolution limits the minimum detectable speed - Integration method: Forward Euler vs. Tustin bilinear transform changes the stability boundary
AMR example: The STM32 motor control runs at 10 kHz. At this rate, the electrical time constant (0.5 ms = 2 kHz bandwidth) is adequately sampled. But if someone reduces the rate to 1 kHz (100 µs → 1 ms) “to save CPU,” the current loop becomes unstable because the electrical dynamics aren’t sampled fast enough.
To faithfully capture a signal of frequency $f$, you must sample at rate $f_s \geq 2f$.
For control systems, the rule is stricter:
$$f_s \geq 10 \times f_{BW}$$
where $f_{BW}$ is the closed-loop bandwidth. This is because: - Shannon says 2× is the minimum to avoid aliasing - But a controller needs to react to the signal, not just record it - 10× gives enough samples per cycle for the controller to effectively act
robot motor control sampling rates:
| Loop | Bandwidth | Min sample rate (10×) | Actual AMR rate | Margin |
|---|---|---|---|---|
| Current | ~2 kHz | 20 kHz | 20 kHz | 1× (tight!) |
| Speed | ~50 Hz | 500 Hz | 10 kHz | 20× (comfortable) |
| Position | ~5 Hz | 50 Hz | 1 kHz | 20× (comfortable) |
| Nav2 cmd_vel | ~2 Hz | 20 Hz | 50 Hz | 2.5× (adequate) |
If you sample a 300 Hz vibration at 500 Hz, it appears as a 200 Hz signal (500 - 300 = 200). The controller “sees” a phantom 200 Hz disturbance and tries to reject it — making things worse.
AMR example: Motor cogging at 200 Hz + speed loop at 10 kHz = no problem (10000 >> 2×200). But if the SPI bus delays speed feedback to an effective 300 Hz rate, the cogging can alias into the controller’s useful bandwidth.
Solution: Anti-alias filter before sampling. A simple first-order low-pass filter:
$$H(s) = \frac{1}{\tau_f s + 1}, \quad \tau_f = \frac{1}{2\pi f_{cutoff}}$$
Set $f_{cutoff} = f_s / 4$ to prevent aliasing while preserving useful bandwidth.
The continuous PID controller:
$$C(s) = K_p + \frac{K_i}{s} + K_d s$$
Must be converted to a discrete-time difference equation that a microcontroller can execute every $T_s$ seconds.
The question: How do you approximate the continuous integral and derivative?
Replace: $s \approx \frac{z - 1}{T_s}$
Integral:
$$\int e \,dt \approx \sum_{k=0}^{n} e[k] \cdot T_s$$
or equivalently: $I[n] = I[n-1] + T_s \cdot e[n]$
Derivative:
$$\frac{de}{dt} \approx \frac{e[n] - e[n-1]}{T_s}$$
Full discrete PID (Forward Euler):
// Positional form
float pid_compute(float error, float dt) {
integral += error * dt; // Forward Euler integration
float derivative = (error - prev_error) / dt; // Forward Euler derivative
prev_error = error;
return Kp * error + Ki * integral + Kd * derivative;
}
Pros: Dead simple, one multiply-add per term. Cons: Can be unstable for fast dynamics. The stability region of Forward Euler is a circle of radius 1 centered at (-1, 0) in the z-plane. If the motor’s poles lie outside this circle at your sample rate, the discrete controller is unstable even though the continuous one is stable.
Replace: $s \approx \frac{z - 1}{T_s \cdot z}$
Integral:
$$I[n] = I[n-1] + T_s \cdot e[n]$$
(Same equation, but it’s derived differently and has better stability properties.)
Derivative:
$$D[n] = \frac{e[n] - e[n-1]}{T_s}$$
Backward Euler is unconditionally stable for stable continuous systems — it never introduces instability through discretization. But it’s more damped than the continuous system (slightly sluggish).
Replace: $s \approx \frac{2}{T_s} \cdot \frac{z - 1}{z + 1}$
This maps the left half of the s-plane exactly to the interior of the unit circle in the z-plane. Stability is preserved perfectly.
Tustin integral:
$$I[n] = I[n-1] + \frac{T_s}{2}(e[n] + e[n-1])$$
This is the trapezoidal rule — average of current and previous error × time step. Much more accurate than Forward Euler for the same sample rate.
Tustin derivative (with filter):
$$D[n] = \frac{2K_d}{2\tau_f + T_s} (e[n] - e[n-1]) + \frac{2\tau_f - T_s}{2\tau_f + T_s} D[n-1]$$
where $\tau_f = K_d / (N \cdot K_p)$ is the derivative filter time constant.
Full Tustin PID:
float pid_compute_tustin(float error) {
// Proportional
float P = Kp * error;
// Integral (trapezoidal)
integral += 0.5f * Ki * Ts * (error + prev_error);
// Derivative (filtered, Tustin)
float c1 = 2.0f * Kd / (2.0f * tau_f + Ts);
float c2 = (2.0f * tau_f - Ts) / (2.0f * tau_f + Ts);
deriv = c1 * (error - prev_error) + c2 * prev_deriv;
prev_error = error;
prev_deriv = deriv;
return P + integral + deriv;
}
robot firmware uses Tustin for the speed loop because it’s the best accuracy-to-cost ratio. The current loop uses Forward Euler because at 20 kHz, even Forward Euler is accurate enough (the electrical time constant is only ~1 ms = 20 samples).
| Method | Stability | Accuracy | Computation | When to use |
|---|---|---|---|---|
| Forward Euler | Can destabilize | Low | 1 multiply + 1 add | Very fast sample rate (>20× bandwidth) |
| Backward Euler | Always stable | Low-Medium | 1 multiply + 1 add | Safety-critical, slow sample rate |
| Tustin (Bilinear) | Preserves stability | High | 2 multiplies + 2 adds | Default choice for motor control |
| ZOH (exact) | Preserves stability | Exact | Matrix exponential | Simulation only (too expensive for MCU) |
The positional form computes the absolute output:
$$u[n] = K_p e[n] + K_i \sum e[k] T_s + K_d \frac{e[n] - e[n-1]}{T_s}$$
The velocity form computes the change in output:
$$\Delta u[n] = u[n] - u[n-1] = K_p(e[n] - e[n-1]) + K_i T_s \cdot e[n] + K_d \frac{e[n] - 2e[n-1] + e[n-2]}{T_s}$$
Then: $u[n] = u[n-1] + \Delta u[n]$
robot firmware velocity-form PID:
typedef struct {
float Kp, Ki, Kd;
float Ts; // Sample period
float prev_error;
float prev_prev_error;
float output; // Accumulated output
float output_min, output_max;
} PID_VelocityForm;
float pid_velocity_compute(PID_VelocityForm *pid, float error) {
float delta_u = pid->Kp * (error - pid->prev_error)
+ pid->Ki * pid->Ts * error
+ pid->Kd / pid->Ts * (error - 2.0f * pid->prev_error + pid->prev_prev_error);
pid->output += delta_u;
// Clamp (inherent anti-windup)
if (pid->output > pid->output_max) pid->output = pid->output_max;
if (pid->output < pid->output_min) pid->output = pid->output_min;
pid->prev_prev_error = pid->prev_error;
pid->prev_error = error;
return pid->output;
}
A quadrature encoder with $N$ counts per revolution gives:
$$\Delta\theta_{min} = \frac{2\pi}{4N} \text{ rad (with 4× decoding)}$$
Speed from encoder at sample rate $f_s$:
$$\omega[n] = \frac{\theta[n] - \theta[n-1]}{T_s} = \frac{\Delta\theta \cdot \text{count_diff}}{T_s}$$
Minimum detectable speed:
$$\omega_{min} = \frac{2\pi}{4N \cdot T_s}$$
AMR encoder: $N = 512$ counts/rev, 4× decoding = 2048 edges/rev, $f_s = 10$ kHz:
$$\omega_{min} = \frac{2\pi}{2048 \times 0.0001} = 30.7 \text{ rad/s} = 293 \text{ RPM}$$
That’s a terrible resolution for low-speed control! At the wheel (after 50:1 gearbox), the minimum detectable speed is 30.7/50 = 0.61 rad/s. For the robot wheel (radius ~0.065 m), that’s 0.04 m/s.
Instead of computing speed every 100 µs (10 kHz), compute it every 1 ms (1 kHz) but still run the PID at 10 kHz with the most recent speed estimate.
$$\omega_{min} \text{@ 1 kHz} = \frac{2\pi}{2048 \times 0.001} = 3.07 \text{ rad/s}$$
10× better resolution, but 10× more delay in the speed estimate.
Average the count differences over $M$ samples:
$$\omega[n] = \frac{1}{M \cdot T_s} \sum_{k=0}^{M-1} (\theta[n-k] - \theta[n-k-1])$$
Equivalently: $\omega[n] = \frac{\theta[n] - \theta[n-M]}{M \cdot T_s}$
This is a FIR low-pass filter. $M = 10$ gives 10× better resolution with only $M \cdot T_s / 2 = 0.5$ ms group delay.
Instead of counting edges per time, measure time per edge. At low speed, this gives much better resolution. At high speed, revert to counting.
robot firmware uses a hybrid: Edge counting at high speed (>100 RPM) and period measurement at low speed (<100 RPM), with crossover blending.
The current sensor’s ADC has $n$-bit resolution:
$$\Delta I_{min} = \frac{I_{range}}{2^n}$$
For AMR: 12-bit ADC, ±10A range → $\Delta I = 20/4096 = 4.9$ mA per LSB. This is adequate for current control.
| Higher sample rate | Lower sample rate |
|---|---|
| ✅ Better accuracy | ✅ Less CPU load |
| ✅ More stability margin | ✅ More time for computation |
| ✅ Less aliasing | ✅ Better encoder resolution |
| ❌ More CPU load | ❌ Risk of aliasing |
| ❌ Worse encoder resolution (count diff = 0 or 1) | ❌ Risk of instability |
1. Determine the fastest dynamic you need to control
→ AMR: electrical time constant = 0.5 ms → f_elec = 2 kHz
2. Multiply by 10-20× for the minimum sample rate
→ f_s_min = 20 kHz for current loop
→ f_s_min = 500 Hz for speed loop (bandwidth ~50 Hz)
3. Check encoder resolution at that rate
→ 20 kHz with 2048 CPR = ω_min = 61 rad/s (too coarse for speed)
→ Use period measurement or lower-rate speed computation
4. Check CPU budget
→ STM32F4 @ 168 MHz: ~8400 cycles per 20 kHz interrupt
→ PID computation: ~50-100 cycles
→ Plenty of headroom
5. Choose the highest rate your CPU can sustain
→ Current loop: 20 kHz
→ Speed loop: 10 kHz (or 1 kHz speed estimate feeding 10 kHz PID)
I[n] = I[n-1] + Ts * e[n]. Write the Tustin version.