Closing three loops on an AKM11E servo
Three nested FOC loops (current 20kHz, velocity 5kHz, position 1kHz) on a bare metal STM32F411 with a DRV8353RS gate driver, driving a ball screw linear stage. Every loop is designed from measured frequency response data.
Purpose and approach
Build and verify a single axis embedded servo controller like the ones used in industrial precision motion. The project started from a conversation with a former coworker at a wafer dicing company, whose team is thinking about building their own motion controller for a machine with an XYθ stage and a laser galvo. This drive is a single axis proof of concept for that, with the usual industrial targets: a 20kHz current loop update and a current loop crossover around 500Hz.
The loops are closed from the inside out: current, velocity, then position. Each one follows the same three steps: measure the plant with a frequency sweep on the hardware, design the controller against that measurement, then sweep the closed loop to check the result. Each outer loop targets 60° of phase margin and should be three to five times slower than the loop inside it. The data is analyzed offline in Python.
With the loops closed, the stage friction is measured and fed forward, and the drive runs real trapezoidal moves commanded from the Pi.
Hardware & firmware architecture
Hardware and firmware built from scratch: STM32F411, TI DRV8353RS gate driver, Raspberry Pi 5 for telemetry and analysis. No vendor motor control SDK.
TIM1 runs 20kHz center aligned PWM. The same timer triggers the ADC at the counter peak, the middle of the low side on time, which is when the shunts see phase current. The encoder is read in hardware by TIM2.
Current loop at 20kHz, velocity at 5kHz, position at 1kHz. The DRV8353 is configured over SPI1. SPI2 streams telemetry to the Pi, which does the analysis in Python.
Current loop
A voltage chirp on vq, measure iq, and fit P(s) = 1/(Ls+R).
Fitted R is within 0.7% of the datasheet (1.54Ω vs 1.55Ω), which says the current sensing chain is right. L reads 17% high (1.20mH vs 1.02mH); the cause is still open (see open items), so the design uses the closed loop values.
The PI zero cancels the plant pole (Ki/Kp = R/L), so Kp sets the crossover. Gains use R=1.73Ω and L=1.05mH from a closed loop sweep.
The crossover stays at 500Hz for now: the 5kHz telemetry cannot verify a faster loop (see open items).
A chirp on iq,cmd with the PI closed gives the closed loop response. Loop gain comes from L = H/(1-H).
Crossover 504Hz against the 500Hz design, 72.7° phase margin. Crossover and bandwidth are not the same number: −3dB lands at 767Hz, about 1.5× crossover.
Working back from the measured crossover gives L = 1.04mH, close to the datasheet 1.02mH and the design value 1.05mH.
Current noise at standstill is about 8mA RMS (2.8 ADC counts).
Velocity loop
From here on the motor drives the linear stage (150mm travel, 2mm lead). An open loop sweep did not work below about 10Hz because of friction, so a light P only loop was closed and the plant backed out of it: P(s) = H/(1−H)/Kp.
PI zero on the plant pole (11.2Hz). Kp raised until phase margin was about 67°.
Friction feedforward
The velocity loop's frequency response changed with sweep amplitude (8 vs 15 rad/s). Friction in the ball screw and linear bearings is not linear, so the response depends on how hard the loop is driven. This section measures the friction and feeds it forward.
Hold constant speeds from 1 to 23 rad/s in both directions and record the current at each one.
Added Coulomb plus viscous friction as feedforward in the velocity loop, using tanh() so it does not chatter at zero speed, then reran the 8 vs 15 rad/s sweep.
With feedforward the 8 and 15 rad/s sweeps match within 0.02dB at DC, so the response no longer depends on amplitude. The deployed loops run with it on.
Position loop
A P controller around the closed velocity loop. Position is the integral of velocity, so the loop gain is L(s) = Kp · Hvel(s) / s. The integrator adds 90° of lag, which makes the phase margin 90° minus the velocity loop's lag at crossover.
The measured velocity loop lags about 29° at 60Hz, so Kp was set for a 60Hz crossover. The sweep below measures 61.2Hz with 61.2° of margin.
† At 0.10 rad drive. The −3dB number drops if the loop is driven into its clamps; crossover and margin do not.
Trajectory tracking
The Pi sends one MoveCmd per move over SPI, and the STM32 generates the trapezoidal profile and runs it.
With friction fed forward, the error left at cruise is the v/Kp lag of a P position loop: 38.4µm at 10mm/s. Velocity feedforward removes it by passing the generator's speed straight to the velocity loop. The move below is a 10mm trapezoid at 10mm/s and 250mm/s² with both feedforwards on.
Conclusion
Every loop was designed from a measured plant and checked with a separate closed loop measurement.
| Loop | Rate | Bandwidth (−3dB) | Phase margin |
|---|---|---|---|
| Current | 20kHz | 767 Hz | 72.7° |
| Velocity | 5kHz | 171 Hz | 67.0° |
| Position | 1kHz | 123.8 Hz | 61.2° |
Friction feedforward removed the amplitude dependence, and velocity feedforward cut cruise following error from 38.4µm to within ±0.73µm. The same process applies to any servo axis: measure the plant, design to it, measure the result.
Open items
- Current loop bandwidth: raise telemetry above 5kHz (needs a DMA ring on the STM32) and use it to push the current loop above 500Hz.
- Velocity observer: implemented and running in the velocity loop, not yet written up here.
- Minimum jerk move profile (quintic, selected with TRAJ_PROFILE): implemented in the firmware, not yet written up here.
- Inductance: open loop sweep reads 1.20mH vs 1.05mH closed loop. Check how much is the cable.
- Acceleration feedforward, for the last ~2µm at the ramp corners.
- Run the control loop from the ADC interrupt and drop the TIM1 interrupt. Measure the delay saved.