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 = \frac{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. (Completeness note: in salient machines (IPM), a reluctance term, ‎(3/2)·p·(L_d − L_q)·i_d·i_q‎, joins Equation 1 and makes i_d useful too; this article assumes the surface-magnet case, where it vanishes.)

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:

$$\begin{aligned} i_\alpha &= i_a \\[4pt] i_\beta &= \frac{i_a + 2\,i_b}{\sqrt{3}} \end{aligned}$$

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. Two caveats keep this honest. First, the model above is continuous-time and ignores sampling delay, deadtime and measurement filtering, all of which eat phase margin as ω_c rises, which is exactly why the one-tenth rule exists; push the bandwidth and the analytic gains stop being a guarantee. Second, if the loop misbehaves at a modest ω_c, the problem is almost always measurement, angle alignment or the current-sampling point, not the absence of hand-tuning. At higher speeds, add the standard decoupling feedforward terms to the PI outputs so the axes stop fighting each other (written with L_d and L_q so it covers salient machines too):

$$v_d^{ff} = -\,\omega_e L_q\, i_q, \qquad v_q^{ff} = \omega_e\,(L_d\, 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. One more convention trap: the vbus * SQRT3_INV in the voltage clamp (v_bus/√3) is correct for the amplitude-invariant scaling used throughout this article. Under a different normalization the constant changes, so audit every constant against the convention you picked.

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.