Precision control of N20 motor: the AriwooMotor library

Hi! If you have been following along, you know the encoder shootout ended with the TMAG5273 on the throne. Reading an angle beautifully is lovely, but Ariwoo does not need a spectator - it needs a wheel that goes exactly where it is told. This log is the next layer: the motor control library. It is the longest log I have written, because a few of you asked me to explain EVERY term in the code, so that is exactly what I am going to do. Grab a coffee. ♥️

A small note before we start: the diagrams in this log are drawn inline as SVG, so you can hover over the parts for little tooltips, and the deeper math lives in fold-out sections you can click open if you want the full story. Skip them freely - the log reads fine without them.

The code being explained today (also linked at the bottom), all three files in one zip:

AriwooMotorControl.zip - containing:

1. AriwooMotorControl.ino - the thin sketch
2. AriwooMotor.h - the library interface (read this one first, the comments are half the documentation)
3. AriwooMotor.cpp - the implementation

And as always: I use Arduino for rapid prototyping because it gives me conceptual clarity faster. Production firmware will be ESP-IDF - and this library was deliberately written so that only ONE class has to change for that port (more on that below).

What the code will do:

The command I want to type over serial is dead simple:

<delta_degrees> , <rpm>

• 0.5, 1200 → nudge forward 0.5 degrees, allowed to travel up to 1200 rpm
• -360, 1200 → one full turn anticlockwise, up to 1200 rpm
• 720, 1 → two full turns, but crawl at 1 rpm

So the degrees is a DISTANCE (sign = direction) and the rpm is a SPEED LIMIT for the move, not a demand. 0.5, 1200 does not mean “slam into 0.5 degrees at warp speed” - it means “you may go fast if it helps, but land on 0.5 degrees cleanly”.

The four things I refused to compromise on, straight from my own brief:

1. 0.5 degree landing precision. Every move must end within half a degree of where it was told to end.
2. Fine speed control across the whole 1 → 1200 rpm range. Three orders of magnitude with one controller.
3. Modern, fail-proof noise filtering, always read incrementally. No batch processing, no stored samples, every tick digests one reading.
4. Human-readable, maintainable code. Future me (and future ESP-IDF me) must be able to open this in six months and not cry.

The hardware cast

• N20 motor - the tiny geared DC motor on Ariwoo’s wheels. A plain brushed DC motor: put voltage across it, it spins; more voltage, faster spin; reverse the polarity, it reverses.
• DRV8833 - the motor driver chip. Inside it is a dual H-bridge: four electronic switches arranged like the letter H with the motor as the crossbar, which lets current flow through the winding in either direction. You control one motor with two input pins, IN1 and IN2.
• TMAG5273 - our champion 3D Hall-effect sensor, in the exact configuration the previous logs earned: By/Bz axes (the pairing that behaves for my radial magnet), the on-chip CORDIC angle, continuous mode.

CORDIC, if you missed the earlier logs, stands for COordinate Rotation DIgital Computer - a clever shift-and-add algorithm that computes atan2 without a hardware multiplier. The chip runs it internally on two field axes and hands you a finished 0-360 degree angle over I2C.

The big picture

Everything in the library is one straight data pipeline. A raw, slightly noisy angle goes in at the top every tick, and a PWM duty comes out at the bottom:

Seven classes, one per box (plus the tuner). The header file literally contains this diagram in ASCII - I wanted the code to explain itself even without this log.

One design decision up front, because it shapes everything: the library never reads the sensor itself. The sketch reads the TMAG5273 and hands the raw angle to motor.update(rawAngle, micros(), quality). That keeps the library independent of the SparkFun driver, trivially unit-testable, and means the ESP-IDF port only has to replace Drv8833’s guts (the PWM calls) and nothing else. Every other class is plain C++ math with zero Arduino globals inside.

Part 1: Drv8833 - turning a number into torque

The controller ends every tick with one number: a duty in the range −1 to +1. Duty is short for duty cycle - the fraction of time a PWM (Pulse-Width Modulation) signal spends ON. PWM is how you get “60% of the battery voltage” out of a switch that can only be fully on or fully off: switch it on and off very fast, and the motor’s inductance averages it out. Here the switching runs at 25 kHz, deliberately above human hearing, so the motor does not whine at you.

On the ESP32 the PWM comes from the LEDC peripheral (the “LED Controller” - named for driving LEDs, but it is just a very good PWM generator). With 10-bit resolution the duty is set in counts from 0 to 1023 (2¹⁰ − 1), which is why the code converts a 0..1 fraction into counts:

void Drv8833::writeChannel(uint8_t pin, float dutyFraction) {
  dutyFraction = clampf(dutyFraction, 0.0f, 1.0f);
  uint32_t counts = (uint32_t)(dutyFraction * (float)_maxCount + 0.5f);
  ledcWrite(pin, counts);
}

(The + 0.5f is just rounding-to-nearest instead of truncating. And note this is the Arduino core 3.x LEDC API - ledcAttach straight onto a pin; on core 2.x it was ledcSetup + ledcAttachPin. The comment in the code marks exactly where to swap.)

The decision: slow decay, and why the 1 rpm end demanded it

Here is a term that cost me a proper reading session: decay mode. When the PWM switches OFF for its off-portion of the cycle, the current already flowing in the motor winding has to go SOMEWHERE (inductors do not stop on a dime). The two options:

• Fast decay (drive/coast): the winding is left effectively open, the current collapses quickly.
• Slow decay (drive/brake): the winding is shorted through the bridge, the current keeps circulating and decays slowly.

Slow decay keeps the winding energised between PWM edges, which makes torque respond much more linearly at very small duty values. My spec says the motor must crawl at 1 rpm - that lives entirely in the tiny-duty region - so slow decay is the default (cfg.slowDecay = true). This one config flag is quietly one of the most important decisions in the whole file.

Two more terms from this class:

• coast() - both pins low, the motor free-wheels (spin it by hand, it spins).
• brake() - both pins HIGH, the winding is shorted, and the motor’s own generated voltage (back-EMF) fights any motion. An active hold, no power spent. The park mode below leans on this heavily.

Part 2: KalmanAngle - one filter from 1 rpm to 1200 rpm

The raw CORDIC angle is good but jittery. I need two clean numbers out of it: position and velocity. The tool for “estimate a smooth state from noisy measurements” is the Kalman filter, and I have used one before (see my first TMAG log), but this one grew up.

The state vector is x = [theta; omega]:

• theta - the continuous angle in degrees (never wraps - the unwrap stage before it made sure of that),
• omega - the angular velocity in deg/s.

The model is constant velocity: between two ticks we assume the shaft keeps its speed, and any real change in speed is absorbed by a white-noise acceleration term. “White noise” just means random, uncorrelated kicks - we are telling the filter “the acceleration is unpredictable, but here is roughly HOW unpredictable”.

Every tick the filter does two dance steps, forever:

The vocabulary, once, properly:

• Covariance (P) - a little 2×2 matrix in which the filter tracks how UNSURE it is about its own state. Prediction grows it (guessing adds doubt), a measurement shrinks it (evidence removes doubt). The code stores it element-wise (_p00, _p01, _p10, _p11) instead of pulling in a matrix library for four floats.
• Process noise (Q) - how much we allow the model to be wrong, per tick. Built here from the white-noise-acceleration parameter: accelNoiseDegS2.
• Measurement noise (R) - the variance of a single reading, i.e. measNoiseDeg squared. Both knobs have honest physical units on purpose - “degrees of noise per sample” and “deg/s² of surprise” - so tuning them is reasoning, not voodoo.
• Innovation (y) - the surprise: measurement minus prediction.
• Kalman gain (K) - the trust ratio the filter computes fresh every tick: how much of that surprise to accept. Noisy sensor → small gain → lean on the model. Confident model → same. It is the optimal blend, recomputed live.

// ---- Predict: x = F x ----
_theta += _omega * dt;   // omega assumed constant across the step

// ---- Predict covariance: P = F P F^T + Q ----
float p00 = _p00 + dt * (_p10 + _p01) + dt2 * _p11;
float p01 = _p01 + dt * _p11;
float p10 = _p10 + dt * _p11;
float p11 = _p11;

// Q from a white-noise-acceleration model
const float q00 = _qacc * dt4 / 4.0f;
const float q01 = _qacc * dt3 / 2.0f;
const float q11 = _qacc * dt2;

const float y  = measuredContinuousDeg - _theta;   // innovation
const float s  = _p00 + _r;                        // innovation covariance
const float k0 = _p00 / s;                         // Kalman gains
const float k1 = _p10 / s;

_theta += k0 * y;
_omega += k1 * y;

The decision: adaptive noise scheduling (a.k.a. “the noise fix”)

This is the part I am most proud of in this class, and it came straight from pain in the logs. A single fixed Q cannot serve both ends of my spec:

My earlier code picked one value and lived with the ❌. This one refuses to: the controller schedules both knobs live, every tick:

• setAccelNoise() slides Q between accelNoiseRestDegS2 = 120 (rest: heavy smoothing, rock-solid angle) and accelNoiseDegS2 = 1e9 (moving: full tracking bandwidth). That huge moving number is deliberate and sized in the comments: the model must ADMIT the profile’s own acceleration, roughly (25000 deg/s²)². My old value of 40000 allowed per-tick velocity changes of ~0.1 deg/s while the profile demanded ~25 - the filter physically could not follow a move.
• setMeasNoise() slides R the same way: while moving we trust each reading more (1.5°), at rest less (4°). The 4° is honest - the static logs really do show several degrees of raw CORDIC jitter, so my old 0.3° was ~10x optimistic and made the filter chase noise at standstill.

The scheduler itself is a little “motion activity” signal in the facade: it jumps to 1 the instant a speed is commanded and decays back to 0 with a 0.25 s time constant (motionHoldTauSec) after motion stops - the decay tail keeps the bandwidth alive just long enough to flush any tracking lag before the filter clamps back down to quiet mode.

And one hard physical prior: forceStationary(). When the driver is BRAKED, the shaft is not moving - full stop, no debate. So we tell the filter: velocity = 0, kill the cross-covariances. Without this, a leftover velocity estimate makes the model “glide” the position away from a perfectly static measurement for hundreds of milliseconds, and everything downstream chases a ghost. (Found in hardware, obviously. Everything good in this file was found in hardware.)

Part 3: MotionProfile - the polite way to ask for motion

You never hand a controller a step change (“BE at +720 NOW”) - it will slam, overshoot, and your precision dies. Instead you generate a motion profile: a moving setpoint that glides from start to target, and let the controller chase THAT.

Setpoint = the value you are asking a control loop to achieve at this instant. The profile produces two per tick: the position we SHOULD be at, and the velocity we SHOULD have (which becomes feedforward - hold that thought).

The shape is a trapezoid: accelerate at a fixed maxAccelDegS2, cruise at the commanded speed limit, decelerate symmetrically to zero. If the move is too short to ever reach the cruise speed, the same math naturally produces a triangle instead - the profile peaks at vPeak = √(accel × distance) and comes straight back down. One if in plan() decides which:

float tAcc = v / _accel;
float dAcc = 0.5f * _accel * tAcc * tAcc;   // == v^2 / (2a)

if (2.0f * dAcc <= _distance) {
  // Long move: trapezoid with a real cruise phase.
} else {
  // Short move: triangle, we never reach the requested cruise speed.
  _vPeak = sqrtf(_accel * _distance);
}

The decision hiding in here is the whole reason “0.5, 1200” works: because the profile always PLANS the deceleration ramp back to zero, the landing is gentle regardless of the speed limit you shouted at it. Precision does not fall apart just because you allowed 1200 rpm - the profile simply never lets the shaft arrive at the target still carrying speed.

(S-curve / jerk limiting - smoothing the corners of the trapezoid so acceleration itself ramps instead of stepping - is a clean future extension: feed this trapezoid through a jerk filter. I left it as a hook to keep the core obvious and correct.)

Part 4: Pid - the workhorse, with two scars

PID = Proportional, Integral, Derivative. The three-term controller the entire industry runs on:

• P - push proportionally to the current error. The muscle.
• I - accumulate the error over time and push against the leftovers. This is what defeats constant enemies like friction: P alone stalls when its push equals the friction; I keeps growing until the shaft actually moves.
• D - push against the RATE of change of error. The damper that stops overshoot.

Mine is textbook plus two hard-earned modifications:

Scar 1: anti-windup. If the output saturates (hits the ±1 duty rail - the motor simply cannot push harder), a naive integrator keeps accumulating (“integral windup”) and then takes ages to unwind, causing huge overshoot. The fix here is conditional integration: the moment the output clamps, roll back the integral contribution we just added:

if (out > _outMax) {
  out = _outMax;
  _integral -= error * dtSec;   // take it back - it wasn't going anywhere
}

Scar 2: a derivative low-pass filter. The D term differentiates its input, and differentiating noise AMPLIFIES it. Our velocity estimate is clean but not perfect, so raw Kd turns residual jitter straight into PWM chatter. A first-order low-pass (a simple “blend a fraction of the new value in” smoother, time constant velDerivLpfSec = 10 ms) sits between the derivative and the output.

The class is kept deliberately generic - error in, output out, no motor knowledge - “the same code could run a temperature loop tomorrow”, says the header, and it means it.

Part 5: RelayAutoTune - because I refuse to hand-tune a production line

PID gains are motor-specific. Friction, winding resistance, magnet strength - every N20 off the line is slightly different. Hand-tuning each robot is a non-starter, so the library tunes ITSELF at boot, using the classic Åström–Hägglund relay feedback method.

The idea is beautiful: replace the controller with a relay - a bang-bang switch. Below the target speed? Push harder (bias + relayDuty). Above it? Ease off (bias − relayDuty). A motor-shaped system under a relay settles into a steady, self-sustaining oscillation called a limit cycle:

From the oscillation’s amplitude a (half the peak-to-peak speed swing) and period Tu, one formula from describing-function analysis gives the ultimate gain - the proportional gain at which a plain P controller would sit exactly on the edge of instability:

Ku = (4 · d) / (π · a)        d = relay half-amplitude (duty)

And then gains from Tyreus–Luyben tuning rules:

Kp = 0.45 · Ku
Ti = 2.2  · Tu     →  Ki = Kp / Ti
Td = Tu / 6.3      →  Kd = Kp · Td

The decision: Tyreus–Luyben over the more famous Ziegler–Nichols rules. Z-N is aggressive - fast but overshooty and sensitive to plant variation. T-L is deliberately gentler: less overshoot, more robustness margin, which is exactly what you want when every unit off the line is a bit different. A production line does not want the “optimal” tune; it wants the tune that works on all ten thousand motors.

The sneaky second job: the feedforward model

Before oscillating, the tuner runs TWO flat spin-up phases at two different duties and records the (duty, speed) point each settles at. Two points define a line:

duty = ffC0 + ffC1 · speed

• ffC0 ≈ the friction duty - the constant offset just to keep the shaft turning against friction at all,
• ffC1 ≈ the viscous slope - extra duty per extra deg/s (drag that grows with speed).

This tiny model earns its keep three times over. First, it becomes the controller’s feedforward: instead of making the PID discover from scratch how much duty a speed needs (integrator winding up on every single move → overshoot), we PRE-apply the model’s answer and let the PID trim only the residual. Feedforward predicts; feedback corrects. Second, it centres the relay’s bias on the duty the setpoint actually needs on THIS motor - my old fixed bias oscillated wherever the motor felt like, or never crossed the setpoint at all. Third, it even helps the unwrap survive long loop stalls (part 7).

The guardrails (added after watching a simulation melt)

• A 12-second timeout: if no clean limit cycle appears, keep the safe fallback gains and flag TuneTimeout rather than spinning forever.
• An amplitude sanity check: an oscillation too small to measure is an oscillation too small to trust → TuneNoOscillation, fallback gains.
• Physical clamps on the result. A very stiff, low-friction motor makes the relay limit-cycle at the ESTIMATOR’s delay rather than the plant’s, and T-L then returns gains far too hot - seen in sim as a slow 50 Hz duty oscillation at cruise. So: Kp is capped at 5 × ffC1 (anchored to the identified physics, and Ki/Kd scale down with it to keep the T-L ratios), and Ti gets a floor of 0.05 s. Trust the tune, but verify it against the physics you measured.

Part 6: AngleSelfCal - the sensor calibrates itself, still

Carried over from the shootout, refined again. My magnet sits on the flat of a D-shaft (a shaft with one flat side), radially - so the CORDIC angle carries a smooth, repeatable distortion that depends on shaft position. The trick to learning it with no reference encoder:

While the shaft turns at a steady speed, angle vs time must be a straight line. Any repeatable wobble away from that line IS the geometry error.

Terms and decisions, one by one:

• Bin - the 0-360° circle is chopped into 128 slices; each slice accumulates its own average residual. 128 gives ~2.8° per bin - fine enough to capture the distortion’s shape, coarse enough that every bin gets plenty of samples.
• Least-squares fit - the classic “best straight line through points” method. And here is a detail I like: we NEVER store the samples. A slow revolution at a fast loop rate could be tens of thousands of readings, so instead the code keeps just five running sums (n, Σt, Σc, Σt², Σt·c) - exactly enough to solve the line at revolution’s end, at any loop-rate/RPM combination, in constant memory. (Doubles for those global sums, to keep the fit numerically clean.)
• Learn rate (0.10) - each revolution’s table blends only 10% toward the new estimate. An exponential moving average across revolutions, so a single weird revolution cannot poison the table.
• Zero-mean enforcement - after every revolution the table’s average is subtracted out, so the correction can SHAPE the angle curve but can never drift the absolute angle. This is the guardrail that makes self-cal safe to leave on forever.
• Circular smoothing - apply() never reads the raw table; it reads a 7-bin (~20°) circular moving average of it. Real geometry error is smooth (low harmonics) and survives; white per-bin noise gets cut ~2.6× (√7).
• RMS apply threshold (0.4°) - RMS = root-mean-square, the “typical magnitude” of the table. A healthy, well-centred sensor learns nothing but its own noise floor - and applying THAT would warp a perfectly good frame. So the correction stays dormant until the table shows real structure. The boot report prints which way it went: APPLIED - real distortion found or not applied - sensor already clean. I love that line.
• Gating - a revolution only teaches the table if it took a sensible amount of time (0.05-5 s) and had ≥16 samples, so we never learn garbage during acceleration, stall, or direction changes.

The decision that changed from the shootout: learn OPEN-LOOP

In the shootout the self-cal learned during normal running. This library is stricter: learning happens ONLY during a dedicated calibration spin right after the auto-tune - constant duty, zero feedback, 20 revolutions at a gentle 100 rpm, then the table is FROZEN forever.

Why? Because “steady rotation” was doing a lot of work in that assumption up there. Under closed-loop control the controller is constantly micro-correcting the speed, and those corrections leak into the angle-vs-time fit as fake geometry - which the controller then reacts to, which teaches more fake geometry… a feedback loop INSIDE the feedback loop. Open-loop constant duty means the fit sees pure sensor geometry and nothing else. And the spin is slow on purpose: more samples per bin per revolution is what averages the sensor noise out of the table.

Part 7: AriwooMotor - the facade, the state machine, and the paranoia

Facade is the design-pattern word for “one friendly class that hides the orchestra”. The sketch only ever talks to AriwooMotor; everything above gets wired together inside. It runs a state machine - the code is always in exactly one named state, with explicit transitions:

Let me walk the states, because each one hides at least one decision worth explaining.

Probing: “which way is forward?”

Before anything else, a 150 ms open-loop nudge at 0.35 duty, then a look at which way the shaft actually went. If a positive duty moved the angle NEGATIVE, the motor is wired backwards - and instead of asking anyone to swap wires, the library records dirSign = −1 and silently flips every future command through one function:

void applyEffort(float effort) {
  float raw = ariwoo::clampf(_dirSign * effort, -1.0f, 1.0f);
  _drv.setDuty(raw);
  _lastDuty = raw;
}

Why is this not just a convenience? Because a reversed motor under feedback control is positive feedback: the controller sees the error growing, pushes harder, error grows faster… the shaft runs away at full duty. The probe kills that whole failure class at boot, per unit, forever. (If the shaft barely moved - jammed, unpowered, who knows - the probe declares itself ProbeInconclusive rather than guessing, and the runaway guard stays on duty during the tune.)

Also filed under paranoia: a runaway guard at 1800 rpm (1.5× the max I ever command) that cuts the duty and latches a fault during tuning and calibration, and the probe deliberately drives the RAW driver (not applyEffort) because it is MEASURING native polarity - correcting the thing you are trying to measure would be a fine bug.

The unwrap, and why it got velocity-aided

Unwrapping: the sensor reports 0-360° and jumps 359.9 → 0.1 at the crossing. The classic fix assumes each tick moves less than 180° and picks the small step. But do the numbers: at 1200 rpm the shaft covers 180° in 25 milliseconds. One blocked Serial.print (see part 8…) and the assumption breaks - the unwrap silently loses half a revolution, and your “absolute position” is now a lie, forever.

So this unwrap asks the Kalman filter first: “given the current velocity, where SHOULD the shaft be?” and then picks the ±360k candidate closest to that prediction. And for really long gaps there is one more layer: the stale velocity under-predicts if the shaft was still accelerating during the stall (the PWM keeps driving even while the CPU is stuck!), so the prediction blends toward the steady-state speed the identified motor model (ffC0/ffC1 again) expects for the duty that was left applied. Defence in depth for a corner case that WILL eventually happen on real hardware.

The field-quality gate: trusting the sensor by how loudly it speaks

The sketch reads not just the angle but the field magnitude |B| every tick and converts it to a 0-1 quality value (fading out below MAG_GOOD = 10, flooring at MAG_FLOOR = 2). Why: an off-centred magnet makes the field COLLAPSE at some shaft angles - the logs showed a good magnet reading ~12-18 everywhere while a bad seating dips to ~0-1 at certain positions - and where the field is dead, the CORDIC angle is garbage.

The quality feeds the estimator through the measurement noise: R = rMeas / quality, and since R is a variance (squared), quality = 0.05 makes a single reading 400× less trusted. The filter then coasts on its model through the dead zone and picks the measurement back up on the other side. The velocity-loop error is attenuated the same way (× (0.10 + 0.90 · quality)) - softly, never frozen, so the loop stays closed against real, persistent divergence. Without this gate the estimator chased the garbage: velocity whipsaw and a duty slam once per revolution, right where the field died.

Moving: the cascade in action

Cascaded control = a control loop whose output is the setpoint of another loop. The outer position loop is a plain proportional controller: position error × positionKp (20 /s) = a velocity command. Add the profile’s own velocity as feedforward - so we track the moving setpoint rather than perpetually chase it from behind. The inner velocity loop is the auto-tuned PID, riding on top of the identified duty = ffC0 + ffC1·v model:

float velCmd = sp.velocityDegS
             + _cfg.positionKp * (sp.positionDeg - _kf.positionDeg());
velCmd = clampf(velCmd, -vmax, vmax);

float velErr = (velCmd - _kf.velocityDegS()) * (0.10f + 0.90f * _qualitySample);
float ff = _ffC1 * velCmd + _ffC0 * clampf(velCmd / 8.0f, -1.0f, 1.0f);
float effort = ff + _velPid.update(velErr, dtSec);

Why two loops instead of one position PID? Separation of concerns, in control-theory clothing: the inner loop soaks up the motor’s ugly nonlinear physics (friction, load changes) at high bandwidth, so the outer loop sees an almost-ideal “velocity servo” and can stay a simple, honest P gain. It is also exactly the structure the relay tuner knows how to tune. (That clampf(velCmd / 8.0f, ...) on the friction term is a soft sign function - a hard sign() would chatter the duty every time the velocity command wobbled around zero.)

One subtle timing decision lives here too: the Kalman filter always receives the TRUE elapsed time (so its covariance grows honestly across a long gap), but the control loop receives a clamped copy (maxCtrlDtSec = 20 ms) - otherwise one serial/I2C stall dumps a giant time step into the PID integral and the duty jumps. Same clock, two different consumers, two different needs.

Holding: where the 0.5 degree promise gets kept

Here is the uncomfortable truth this file had to face: continuous PID cannot park a motor through stiction. Stiction (static friction) is the extra grip that holds a stationary shaft: below some duty, NOTHING moves. So near the target the integrator winds up… up… reaches breakaway… the shaft HOPS - straight past a 0.25° window - the error flips sign, and the whole dance repeats, mirrored. A limit cycle around the target, humming forever. No gain tuning fixes it; it is physics.

So inside the park window (±5°, with hysteresis - the exit window is 1.5× the entry window, so noise cannot flap the mode) control changes character completely. Park mode is a discrete measure-kick-measure loop:

1. Brake. Shaft verifiably stationary (and forceStationary() tells the filter so).
2. Measure, patiently - a slow EMA of the RAW angle, dwelling up to 400 ms when close, because the final verdicts need the most averaging.
3. Within tolerance? Settled. Latch it, report the landing, stay braked.
4. If not: one short kick toward the target - length scaled to the remaining error (a 5° handover needs a real shove, the last 0.3° needs a 4 ms tap), strength adapted around the measured breakaway duty (1.3 × ffC0 as the opening bid). Then back to 1.

The adaptation judges sustained progress, not single hops - in noisy sensor regions the measurement EMA jitters more than a small real hop, and per-kick judgement once produced dozens of impotent kicks at constant error. Overshot past the target? Duty × 0.7. Three kicks with no real progress? Duty × 1.3. And two honesty rules: progressive acceptance can widen the tolerance slightly after 8 stubborn kicks but NEVER past 2× - so “settled” always still means the 0.5° spec with my 0.25° setting - and the kick budget caps blind kicking in field dead zones, because every unverifiable kick is real motion the sensor cannot see, walking the shaft AWAY from the target. Better to brake and hold the best verified position than to gamble it.

float posErr    = _target - _kf.positionDeg();   // from the Kalman ESTIMATE
float posErrRaw = _target - _continuous;         // from the raw MEASUREMENT

Part 8: the sketch, and the bug that taught me about baud rates

AriwooMotorControl.ino is deliberately thin - bring-up, serial parsing, telemetry - because everything real lives in the library where the ESP-IDF port can lift it wholesale. But it still contains two lessons that cost me real debugging time.

Never block the control loop for a print

The old sketch printed telemetry at 100 Hz and the control loop mysteriously stumbled at high rpm. The arithmetic, once I finally did it: 115200 baud (bits per second, ~11.5 kB/s of actual bytes) versus ~140 bytes per telemetry line × 100 Hz = 14 kB/s. The TX buffer filled, and Serial.printf silently BLOCKED until space appeared - and one blocked print at 1200 rpm (180° per 25 ms, remember) was enough to alias the unwrap and lose half-revolutions. The fix is two independent layers:

if (now - lastLogUs >= LOG_INTERVAL_US && Serial.availableForWrite() >= 160) {

20 Hz (~2.8 kB/s, comfortable) AND a check that the buffer can take the whole line right now - if not, skip this print entirely; the next tick prints instead. A skipped log line is nothing. A blocked control loop is a lie in your position estimate.

Respect the physics of the angle resolution

The CORDIC angle is only as fine as the field it is computed from. The sensor digitises at 820 LSB/mT (LSB = least significant bit, the smallest step the ADC can represent; mT = millitesla, the field unit), so the angle’s quantisation step is roughly atan(1 LSB / |B|). At |B| = 1 mT that is ~0.07°; at 0.2 mT it is ~0.35° and the raw angle visibly “sticks” between values - and no firmware, no filter, NOTHING can land 0.5° on top of that. So the sketch checks the field at boot and prints the step size it implies, and you can send f over serial to re-check live while physically repositioning the magnet. Fix the magnet first - some lessons just keep being true.

Two register decisions in the same spirit: setConvAvg(TMAG5273_X16_CONVERSION) makes the chip average 16 conversions per reported sample, cutting noise by √16 = 4× at the SOURCE (the static logs showed ±6-8° of raw jitter without it), while still converting in well under a millisecond - at 1200 rpm the shaft only travels ~7° per sample, nowhere near the unwrap limit. And the channel config is verified ONCE at boot, not with an extra I2C register read inside every control-loop iteration like my earlier logging sketches did - the hot loop stays lean.

The boot report ties the whole story together on one screen:

// direction probe: dirSign = +1  (normal polarity)
// tune result: Kp=0.00291 Ki=0.01096 Kd=0.000141  fault: none
// motor model: friction duty=0.0812  viscous=1.02e-04 duty/(deg/s)
// field over rotation: mag min=11.2 max=17.9
// self-cal: 20 revs learned, table rms=0.612 deg (APPLIED - real distortion found)

Every number in there was MEASURED on this specific unit, this specific boot. Nothing hand-tuned, nothing stored in flash. That was the whole point.

Every decision on one table

Closing thoughts

The encoder logs were about learning to SEE. This one was about learning to ACT - and it turns out acting precisely is mostly about being honest with yourself: honest about how noisy the sensor really is (schedule R to reality, not to hope), honest about what physics allows (stiction will not be PID’d away, a dead field cannot be filtered into existence), and honest in what you report (the landing receipt comes from the raw measurement, always).

And my favourite meta-lesson: almost every hard-won fix in this file - the noise scheduling, the ghost-glide split, the blocked-print unwrap bug, the impotent-kick adaptation - was found by READING MY OWN LOGS. Log everything, peeps. The logs are the debugger.

Next up: two of these on one I2C bus (the whole reason the TMAG5273 won!), and then this library meets the balance controller. That is going to be a fun log.

The code, once more: AriwooMotorControl.zip (the sketch, AriwooMotor.h, and AriwooMotor.cpp, all in one)

Thank you for reading all the way down here! ♥️