Keeps the robot upright, and knows when it has stopped being able to.
Runs on every lean sample — the IMU's 100 Hz sets the loop rate — and drives both wheels from a PD law on the lean error. Leaning forwards means driving forwards to get back underneath, so the sign is straightforward: positive error, positive wheel velocity.
Why PD and not something better
The plant is an inverted pendulum on a cart, which has four states: wheel position, wheel velocity, lean and lean rate. There are no encoders, so two of those four are unobservable and the cascaded and full-state-feedback designs that would use them are not available. PD over the two we measure is not a first approximation here, it is the whole of what the hardware supports.
The consequence is worth stating plainly: this will not hold station. Nothing in the loop knows where the robot is or how fast it is going, so it balances while drifting.
The trim, which is doing more work than it looks
Holding the wrong lean means accelerating forever, so setpoint error is not a cosmetic problem — it is the difference between balancing and running away. And the setpoint cannot be measured well enough to avoid it: it comes from the centre of mass, whose fore-aft position is a small number on a 20.7 mm body, and the angle moves 1.22 degrees for every millimetre of error there.
So the setpoint is trimmed while running. At the true balance point the robot needs no net wheel command to stay there, so a persistent average command means the setpoint is wrong, and the trim integrates that average away. It is deliberately slow — seconds against the pendulum's 69 ms — because a trim that moves at the speed of the balance loop is just a badly tuned integrator, and oscillates.
The accelerometer is the other candidate signal and is a worse one: it cannot separate leaning from accelerating, and the AHRS filter absorbs the difference into its attitude estimate, so the signal fades at exactly the frequency the trim cares about.
Once it settles on a flat floor the trim is a better measurement of the centre
of mass than the bench one: x = z * tan(setpoint + trim).
Falling over
Past fall_angle the wheels can't get back underneath, so it brakes them and
transitions to :fallen, where it waits to be picked up. Coming back is
deliberately harder than leaving: the lean has to be within catch_angle of
the setpoint and turning slower than catch_rate. Without the rate
condition it grabs while being carried, because a robot waved about passes
through upright on the way.
Summary
Functions
Whether the robot is being held still enough, and near enough to upright, to take over from whoever is holding it.
The state the robot's current attitude calls for, asked of the running loop.
Which operational state an attitude calls for.
The wheel velocity a lean error and rate call for, in rad/s.
Whether the wheels are telling the trim anything true about the setpoint.
How long the loop is taking to swing back and forth, in seconds.
Which of the two slew rates this tick calls for.
Whether the pilot has just let go of the turn, as opposed to never having asked for one.
What the loop has been doing lately, small enough to read.
Summarise a trace and a list of events. Split out so it can be tested.
A running average of the wheel command, over tau seconds.
The last few seconds of the loop, oldest first.
The trim after a sample, clamped to limit.
The differential wheel velocity a heading error and yaw rate call for, in rad/s.
Functions
Whether the robot is being held still enough, and near enough to upright, to take over from whoever is holding it.
@spec classify(module()) :: :balancing | :fallen | :idle
The state the robot's current attitude calls for, asked of the running loop.
It knows things a caller doesn't: the trim it has wound on, and the last lean
the IMU reported. Returns :idle if there is no balance controller, which on
the host there isn't.
Which operational state an attitude calls for.
The one place that decides this, so that the loop and BB.NSK.Command.Arm
cannot come to different conclusions about the same robot.
The wheel velocity a lean error and rate call for, in rad/s.
Positive is forwards, which is the way to move to get back under a forward lean, so the proportional term takes the error's sign directly.
Whether the wheels are telling the trim anything true about the setpoint.
Only when nobody is driving and the command is small. The trim exists to notice that the robot needs a persistent nudge to stay put and conclude the setpoint is wrong — but a hard command means something else is going on, and integrating it teaches the trim a lie.
Measured, because this was doing real harm. A shove railed the trim inside a second, railed it the other way as the recovery overshot, and left the robot balancing against a fully displaced setpoint for the fifteen seconds it took to unwind — the command that would unwind it being nearly zero once calm. Two shoves, two falls. The separation is clean: calm balancing sits under 0.8 rad/s and a shove produces 4 to 18, so a ceiling of 2 lands in empty space.
How long the loop is taking to swing back and forth, in seconds.
Counts crossings of the mean lean error across a trace and halves the mean
interval between them. That is the period a kd should be set from — Ziegler
and Nichols put it at kp * period / 8 — and eyeballing a wobbling robot will
not give it to you.
Crossings of the mean, not of zero. A robot whose setpoint is wrong wobbles about a lean it shouldn't be holding, so the error oscillates without ever changing sign, and counting zero crossings there measures whatever the trace was doing either side of the oscillation instead.
The median interval, not the mean. A real trace is not a clean sine: it covers wobbling, falling and being picked up, and noise near the mean throws extra crossings. On the robot the intervals ran from 0.015 to 0.624 seconds, so the mean answered 0.299 for a wobble the median and the peaks both put at 0.13. Since a derivative gain comes straight out of this number, the difference is a factor of two in the gain most likely to be causing the oscillation in the first place.
Narrow the trace to a window that is actually oscillating before trusting it; four seconds of a robot's life is rarely one regime.
nil when the error doesn't cross its own mean twice, which means there is no
oscillation to measure.
Which of the two slew rates this tick calls for.
release while the drive lean is on its way back towards zero, slew while it
is on its way out. The asymmetry is the point: a step in the drive lean is
answered with kp times it, and that kick drives the wheels forward under a
body still leaning forward — on the way out it is a lurch, and on the way back
it is the deceleration. So the lean is walked out slowly and brought back
fast, because how quickly letting go stops the robot is what a pilot judges the
pad on.
The condition says the step and the lean point opposite ways, which is the same thing as the lean shrinking. Decided per tick rather than once per command, so a throttle reversed straight through zero is fast down to the balance point and slow away from it — the same rule either side rather than a special case.
Comparing magnitudes instead would get that case wrong: reversing to an equal and opposite throttle leaves them equal, and the whole transit would crawl.
Whether the pilot has just let go of the turn, as opposed to never having asked for one.
The distinction matters because the two want opposite things from the held heading: a robot that was never turning should keep holding the heading it has, and one that has just stopped turning should adopt the heading it actually reached — otherwise the lag it built up during the turn is a standing error the loop goes on correcting after the thumb comes off, turning it further.
A zero turn is compared numerically rather than matched, because a negated zero
deflection is the obvious way to arrive here and -0.0 does not match 0.0.
What the loop has been doing lately, small enough to read.
A trace is four hundred tuples, which nobody can look at. This is the same data as a dozen numbers and a list of things worth knowing, with the angles in degrees because that is how people think about leaning.
:notes is the part that matters. It encodes what we have learned to look for
the hard way — a trim sitting on its clamp, a command that never changes sign,
saturation — so that reading a trace doesn't depend on remembering which of
those means what.
Summarise a trace and a list of events. Split out so it can be tested.
A running average of the wheel command, over tau seconds.
The trim integrates this rather than the command itself. The command is dominated by the loop's own oscillation — swinging tens of rad/s either side of zero several times a second — and integrating that means the trim chases whichever way the last swing went and rails within a second. Averaged over a second the oscillation cancels and what is left is the sustained component, which is the part that means the robot is going somewhere.
The last few seconds of the loop, oldest first.
Each sample is {seconds_ago, lean, error, command, trim}, with the angles in
radians and the command in rad/s of wheel velocity. Gains are found by
watching these rather than by watching the robot: a fall tells you it went
wrong, the trace tells you how.
The trim after a sample, clamped to limit.
A positive command means the robot is driving forwards to hold its lean, so the setpoint is too far forward and the trim goes down to lean it back.
The differential wheel velocity a heading error and yaw rate call for, in rad/s.
Positive turns the robot to its left, which means the right wheel running
faster than the left. error is measured minus held, the same sense as the
lean loop's, and rate is positive turning left.
Negated, where effort/4 is not, and the asymmetry is real rather than a
slip. A lean error is corrected by driving towards it, because that is how
the wheels get back under a falling body. A heading error has no such
inversion: being turned too far left is corrected by turning right.
Clamped, because wheel velocity spent on heading is velocity unavailable for staying upright, and the balance loop already saturates occasionally. A robot that drives straight into the floor has not been improved.