FOC Position Control

IMPORTANT — Implementation-blind document: This document describes behavior, structure, and responsibilities WITHOUT referencing code. No code blocks using programming languages (C++, C, Python, CMake, shell, etc.) are allowed. Use Mermaid diagrams to express behavior instead. Prose descriptions of algorithms are encouraged; source-level details are not.

Diagrams: All visuals must be either a Mermaid fenced code block (```mermaid) or ASCII art inline in the document. External image references using Markdown image syntax are not allowed.

Responsibilities

Is responsible for: - Executing the FOC cascade: an outermost position loop and a middle speed loop (both at 1 kHz within the same LPI callback), and the innermost current control loop (20 kHz) - Regulating rotor mechanical position to a commanded setpoint in radians - Dispatching to the selected position algorithm and routing its output to either the speed loop or, for the state feedback laws, straight to the current loop - Estimating rotor angular velocity from consecutive mechanical angle samples (same wrap-around compensation as speed control) for the middle loop - Resetting the position, speed and current controller state on Enable / Disable - Accepting a position setpoint in mechanical radians and propagating all derived setpoints through the cascade

Is NOT responsible for: - Reading phase currents or encoder position directly from hardware — these are supplied by the Runner - Writing duty cycles to the PWM hardware — duty cycles are returned to the Runner - Multi-turn position tracking — the setpoint is a single-turn mechanical angle, and the loop always takes the shorter way round to it - Flux weakening (Id is always commanded to 0 A)

Component Details

Three-Loop Cascade Architecture

Position control extends the speed control design by adding a third outermost loop. All three loops share the same LowPriorityInterrupt trigger; the outer two loops execute sequentially within a single LPI callback, while the innermost loop executes in the high-priority 20 kHz ISR.

The position loop fires before the speed loop within the same LPI callback. This ensures the speed setpoint written by the position PID is immediately consumed by the speed PID in the same interrupt invocation, keeping both derived setpoints in step.

Outer Loop — Selectable Position Law

The position error is always the wrapped difference between the setpoint and the current mechanical angle θm, folded into [−π, π] so a move across the encoder seam takes the shorter way round rather than most of a turn backwards.

PositionControllerSelector holds every position algorithm in a std::variant and dispatches through std::visit, exactly as the current and speed loops do. Each algorithm declares the kind of output it produces:

• Speed reference (PID, Cascade P, Two-DOF): the value is written as the speed setpoint and the speed loop runs immediately afterwards in the same callback, as it always has.
• Current reference (LQR, LQI): the speed loop is skipped entirely and the value becomes the q-axis current setpoint directly. These laws carry ω in their own state vector and already regulate it, so running them on top of a speed loop would stack two regulators on the same state.

The speed-reference laws saturate at one loop bandwidth of speed per π radians of error. The current-reference laws clamp to ± maxCurrent, and LQI stops accumulating its integral state whenever that clamp is active.

See documentation/design/controller-selection.md for the algorithm set, the numerical conditioning the state feedback designs require, and the rules governing when a selection or a retune is refused.

Middle Loop — Speed Estimation and Speed PID

The speed estimation and speed PID in the middle loop are functionally identical to those described in the FOC Speed Control design document. The speed PID output is clamped to ± maxCurrent (Ampere) and written as the Iq setpoint for the inner current loop.

When a speed-reference position law is active, the speed setpoint consumed by the speed PID is the output of the position law from the same LPI invocation, not a value set by the application. When a current-reference law is active the speed loop does not run at all.

Inner Loop — Torque and Current Control

The inner loop is functionally identical to the FOC Torque Control design document: Clarke, Park, d-axis PID, q-axis PID, inverse Park, and SVM. It runs independently at 20 kHz and reads its Iq setpoint from the value most recently written by the middle loop.

The d-axis setpoint is 0 A, except while an online electrical estimator is attached: the cascade then adds a ±2.5 % of maxCurrent, 10 Hz square wave so the winding resistance stays observable (see service-electrical-ident.md, Excitation).

Enable and Disable

Enable: resets and arms all four PID controllers in cascade order (position, speed, d-axis, q-axis). The last position setpoint is preserved.

Disable: disarms all four PIDs. Calculate() returns zero duty cycles until re-enabled.

LPI Callback Execution Order Within One 1 kHz Tick

Within a single LPI callback, the position and speed loops execute in strict order:

1. Position law fires first: reads θm, computes the wrapped position error, produces either a speed setpoint or an Iq setpoint.
2. Speed PID fires second, if the position law produced a speed setpoint: reads the freshly updated setpoint, computes speed error using Δθ/Δt, produces and clamps the Iq setpoint. A current-reference law writes the Iq setpoint itself and this step is skipped.
3. Callback returns. On the next 20 kHz ISR cycle, the inner loop picks up the updated Iq setpoint.

This ordering guarantees that the Iq setpoint used by the inner loop is always the one derived in the most recent outer-loop tick.

Interfaces

Provided

Required

Data Model

Sequence Diagrams

Single 1 kHz LPI Tick — Position and Speed Outer Loops

Block Diagram

Constraints & Limitations