Position Loop — P (Baseline)

Theory document: Explains the mathematical and engineering principles behind a component or algorithm. This document is descriptive — it records the why and how at a scientific level, independent of any specific implementation. Equations use KaTeX (\(inline\) and \[block\]). Block diagrams use Mermaid or ASCII art.

Overview

The baseline position controller converts position error into a speed reference \(\omega_m^*\) for the inner speed loop. Under cascade separation — inner speed loop bandwidth at least 5× the position bandwidth — the closed-loop position bandwidth equals \(K_p\) directly. No parameter identification is required.

The law is a PI, not the pure P this chapter derives. The derivation below is the P design the cascade shipped with and still the way to read the proportional path; the integral term removes the standing error the P design leaves under a constant velocity reference. Its zero and the weight the proportional term gives the reference are fixed by the design in Integral zero and reference weighting below, not by the loop tunings.

Operates exclusively in the 1 kHz outer handler.

Prerequisites

See documentation/theory/foc-plant-models.md §3 for the position plant derivation.

Mathematical Foundation

Plant Model

Under cascade separation (treating the speed loop as ideal, \(\omega_m \approx \omega_m^*\)), the position dynamics reduce to a pure integrator:

\[ \frac{d\theta_m}{dt} = \omega_m \]

Transfer function from \(\omega_m^*\) to \(\theta_m\):

\[ G_\theta(s) = \frac{1}{s} \]

P Controller Design

A proportional controller \(C(s) = K_p\) closes the integrator loop. This is the proportional path of the implemented PI; see the note in the Overview:

\[ \boxed{\omega_m^*[k] = K_p \cdot \mathrm{wrap}\bigl(\theta_m^*[k] - \theta_m[k]\bigr)} \]

The closed-loop transfer function is:

\[ \frac{\Theta_m(s)}{\Theta_m^*(s)} = \frac{K_p}{s + K_p} \]

The position bandwidth equals the proportional gain directly:

\[ \omega_{bw}^p = K_p \]

Bandwidth selection: Cascade separation requires:

\[ K_p = \omega_{bw}^p \leq \frac{\omega_{bw}^s}{5} \]

Exceeding this limit causes the position loop to drive the speed loop into instability.

Angle-Error Wrapping

Position error must be wrapped to \((-\pi, \pi]\) to ensure the shortest arc is always taken:

\[ e_\theta[k] = \mathrm{wrap}\bigl(\theta_m^*[k] - \theta_m[k]\bigr) \]

\[ \mathrm{wrap}(x) = x - 2\pi\, \mathrm{round}\!\left(\frac{x}{2\pi}\right) \]

Without wrapping, a \(+3.1\) rad reference reached from \(-3.1\) rad would command a \(6.2\) rad (354°) rotation through the long arc instead of the \(0.08\) rad short arc.

Integral zero and reference weighting

The integral zero sits at a fifth of the position bandwidth, \(K_i = 0.2\,\omega_{bw}^p K_p\), and the proportional term acts on \(b\,\theta^* - \theta\) with \(b = 0.75\) rather than on the error:

\[ \omega_m^*[k] = \omega_m^*[k-1] + K_p\left(b\,\Delta\theta^*[k] - \Delta\theta_m[k]\right) + K_i T_s^o\, e_\theta[k] \]

in the velocity form the implementation uses, with every difference wrapped as the error is.

The zero used to sit at the integral weight times the bandwidth, a twentieth of it with the default weight. That places a slow closed-loop pole just beside the zero, and the pair leaves a tail whose size is the residue of that pole: under an ideal speed loop at \(\omega_{bw}^p = 18.85\) rad/s the poles fall at \(-0.995\) and \(-17.9\) rad/s against a zero at \(-0.94\), and a 1.5 rad step keeps some 0.089 rad of overshoot that decays over a second — 0.055 rad is still there half a second later. A torque step pushing the same way as that tail starts its excursion already most of the way across a 0.08 rad recovery band.

Moving the zero up to \(\omega_{bw}^p/5\) alone shortens the tail but makes it larger, because the zero also lies in the reference path. Weighting the reference in the proportional term takes the zero out of the reference path without touching disturbance rejection, which sees the full \(K_p\) through \(\Delta\theta_m\). With \(b = 0.75\) a 1.5 rad step overshoots by well under 1 % and is within 0.01 rad of the setpoint 450 ms after the step.

Output Limiting

The speed reference is clamped to \(\pm\omega_{m,max}\) to prevent demanding speeds beyond the mechanical or electrical limits of the drive:

\[ \omega_m^*[k] = \mathrm{clamp}\bigl(K_p \cdot e_\theta[k],\ -\omega_{m,max},\ \omega_{m,max}\bigr) \]

Numerical Properties

Limitations & Assumptions

• Treats the speed loop as ideal. Valid when \(\omega_{bw}^s \geq 5\, \omega_{bw}^p\).
• Under a constant velocity reference \(\dot{\theta}_m^* \neq 0\), a pure P controller has non-zero steady-state tracking error. The implementation carries an integral term for this, and velocity feedforward (as in P2 / Cascade P) addresses it without the integral’s phase cost.
• No disturbance rejection beyond what the speed loop provides.
• Encoder noise enters the position estimate. A state observer upstream improves robustness.

References

1. Krishnan, R. — Permanent Magnet Synchronous and Brushless DC Motor Drives, CRC Press, 2010.
2. Åström, K.J. & Wittenmark, B. — Computer-Controlled Systems, 3rd ed., Prentice Hall, 1997.