Writing an FOC Controller from Scratch

Hands-on · Motor control firmware · ~13 min read

Field-Oriented Control has a reputation as graduate-level material, yet the entire algorithm is about forty lines of code built on one idea. We've shipped it on everything from a 16 mm board to a 100 V / 30 A industrial drive. This is the article we wish we'd had the first time: the idea, the math you actually need, analytically-derived gains (no trial-and-error), an annotated ISR — and the alignment and calibration gotchas that eat the first two weeks.

Six-step (trapezoidal) commutation drives a BLDC like a light switch: energize two windings, wait for the rotor to cross a Hall boundary, switch. It works — and it's exactly why cheap drives growl at low speed, ripple under load, and waste headroom. FOC instead drives the motor like the synchronous machine it is: keep the stator current vector exactly 90° ahead of the rotor flux, always, smoothly. Improved torque response, stable torque at near-zero speed, four-quadrant operation — the properties on our controller spec sheets all come from this one discipline.

1 · The one idea: torque lives on the q-axis

Seen from the rotor, only the component of stator current that's perpendicular to the rotor's magnetic flux makes torque. For a surface-magnet machine:

$$\tau = \tfrac{3}{2}\,p\,\lambda_{pm}\,i_q$$

Read it twice, because this is the entire game: torque is proportional to i_q. The current along the flux (i_d) makes no torque — for a surface-magnet motor we simply regulate it to zero. So "control the motor" reduces to "control two DC currents": hold i_d = 0, command i_q for the torque you want. The rest of FOC is bookkeeping that makes those two currents be DC.

2 · Getting into the rotor's frame: Clarke and Park

The bookkeeping is two coordinate transforms. The Clarke transform collapses the three 120°-spaced phase currents (only two measured — they sum to zero) into an equivalent two-axis stationary frame:

$$i_\alpha = i_a, \qquad i_\beta = \frac{i_a + 2\,i_b}{\sqrt{3}}$$

The Park transform then rotates that frame by the rotor's electrical angle θₑ, so the axes spin with the rotor:

$$i_d = i_\alpha\cos\theta_e + i_\beta\sin\theta_e, \qquad i_q = -\,i_\alpha\sin\theta_e + i_\beta\cos\theta_e$$

Here's the payoff: in the dq frame, the sinusoidal currents you'd see on a scope become constant values. And constant values are what PI controllers are good at. That's the entire reason these transforms exist.

3 · Two PI loops — with gains you compute, not guess

In the dq frame, each axis of the motor looks (to first order) like a simple resistor-inductor circuit:

$$G(s) = \frac{i(s)}{v(s)} = \frac{1}{L s + R}$$

One pole, known from the datasheet. Place the PI zero on it (pole-zero cancellation), choose a current-loop bandwidth ω_c, and the gains fall out analytically:

$$K_p = \omega_c\,L, \qquad K_i = \omega_c\,R$$

This is the part newcomers don't believe: the current loops are not hand-tuned. Measure or read R and L, pick ω_c (a common choice is one tenth of the switching frequency, in rad/s — e.g. ~1 kHz bandwidth at 20 kHz PWM, ω_c ≈ 6300), compute K_p and K_i, done. If the loop misbehaves after that, your problem is measurement, alignment, or deadtime — not the gains. At higher speeds, add the standard decoupling feedforward so the axes stop fighting each other:

$$v_d \mathrel{-}= \omega_e L\, i_q, \qquad v_q \mathrel{+}= \omega_e\,(L\, i_d + \lambda_{pm})$$

(Everything we wrote about anti-windup and saturation in the PID field guide applies here verbatim — with one twist: the limit is a circle, |v| ≤ v_max, so clamp the vector, q-axis last.)

4 · Back out: inverse Park and SVPWM

The two PI outputs (v_d, v_q) are rotated back to the stationary frame (inverse Park — same as Equation 3 with the angle negated), and then turned into three PWM duty cycles. Use space-vector PWM rather than plain sinusoidal PWM: by riding the hexagonal voltage limit of the inverter instead of the inscribed circle, SVPWM extracts about 15% more usable voltage from the same DC bus — free top speed.

5 · The ISR — the whole algorithm, annotated

Everything above runs once per PWM cycle, typically 10–40 kHz, inside one interrupt:

// FOC core — runs in the ADC end-of-conversion interrupt, every PWM cycle
void foc_isr(void) {
    // 1) Currents: sampled mid-PWM (low-side window), offsets removed
    float ia = adc_a() - offs_a,  ib = adc_b() - offs_b;

    // 2) Electrical angle: encoder counts → mech angle → × pole pairs + offset
    float theta = wrap(enc_angle() * POLE_PAIRS + theta_offset);
    float s = sinf(theta), c = cosf(theta);

    // 3) Clarke + Park: three sinusoids in, two DC values out
    float i_alpha = ia, i_beta = (ia + 2.0f*ib) * INV_SQRT3;
    float id =  i_alpha*c + i_beta*s;
    float iq = -i_alpha*s + i_beta*c;

    // 4) Two PI loops (analytic gains, anti-windup inside)
    float vd = pi_step(&pi_d, id_ref - id);     // id_ref = 0 for SPM
    float vq = pi_step(&pi_q, iq_ref - iq);     // iq_ref = torque command

    // 5) Respect the voltage circle: |v| <= v_max — clamp as a vector
    vlimit_circle(&vd, &vq, vbus * SQRT3_INV);

    // 6) Inverse Park + SVPWM → three compare registers
    float v_alpha = vd*c - vq*s,  v_beta = vd*s + vq*c;
    svpwm_write(v_alpha, v_beta, vbus);
}

Notes that matter: the trig comes from a lookup table or the CORDIC unit on bigger parts; the loop must run at a fixed rate (the gains assume it); and iq_ref is where the outer world plugs in — a speed PI, a position loop, or a torque command straight from a script, which is exactly how our controllers expose it.

6 · The gotchas that eat the first two weeks

The forty lines above are the easy part. These are the field problems:

The first one deserves a word, because everyone hits it: FOC needs the electrical angle of the rotor, and your encoder gives a mechanical angle with an arbitrary zero. The standard alignment move is beautifully dumb — command a fixed voltage on the d-axis only; the rotor snaps to it like a compass needle; whatever the encoder reads at that moment is your offset. Store it, done.

7 · Where this fits in the bigger picture

The current loops you just built are the innermost ring of every motion system we make. Around them sits a speed PI (tuned with the methods from the PID field guide), and around that a position loop — and when many such axes must act as one machine, you're in the territory of our article on precise robot motion. Same discipline at every scale: know the plant, place the gains deliberately, respect the limits explicitly.

From a 16 mm board to 100 V / 30 A

We've implemented this stack — FOC, analytic current loops, SVPWM, script-driven motion on top — on the world's smallest integrated BLDC controller, on medical-grade drives, and on high-power industrial controllers with WiFi. If your product needs a motor to behave — let's talk.

---

Grounded in: Texas Instruments, "Field Orientated Control of 3-Phase AC-Motors" (BPRA073); Microchip AN1078, "Sensorless Field Oriented Control of a PMSM"; the open-source SimpleFOC project and documentation; ST Motor Control SDK documentation. Notation follows the amplitude-invariant convention.