Driving: Motors and Odometry

Programming for Robotics 4 made the robot’s senses trustworthy. This chapter does the same for its body. The robot of Programming for Robotics 1 section 6 had a one-line idea of driving: a command from −1 to +1 for each wheel, handed to set_duty. A real drive needs much more than that. The motor does not turn at all below some duty (the dead zone), and a sudden step in duty is hard on the gearbox, the battery and the wheels’ grip. A “speed” in metres per second is only meaningful once the speed curve of the motors has been measured. A velocity command (go forward at 0.3 m/s while turning at 1 rad/s) has to become two wheel speeds, and if a wheel cannot go that fast, the answer must still be the same circle, only slower. The encoders then tell where the robot has got to, if the pose is updated correctly, and “drive 1 m” has to be a program that stops where it said it would. Finally, every number in the geometry is a little wrong, and the errors can be measured and removed.

The chapter builds all of that as one module, drive.py, in about 900 lines of standard-library Python (about 730 without the tests), in the order the pieces depend on each other. It uses the formulas of Robotics Fundamentals 3 section 2 (the motor model), Robotics Fundamentals 4 sections 3 to 6 (kinematics, odometry and drift), and the safety rules of Robotics Fundamentals 10 section 7, each tied to the code that implements it. It reuses the pieces of the three previous chapters instead of rewriting them: FixedRateLoop and LoopStats of Programming for Robotics 1 sections 2 and 4, Config of its section 6, the simulator of Programming for Robotics 3, and SlewLimiter, SpeedEstimator, Sensor, Health and LinearCalibration of Programming for Robotics 4 (sections 3, 9, 2, 10 and 8). The simulator is the test bench for the whole chapter, and it is made to disagree with the software on purpose several times, so that calibration has something to find.

1. The layers, and the conventions that glue them

The module is a stack, each layer using only the ones above it. Each layer is a small class or function with a test, and the simulator replaces everything below the hardware seam of Programming for Robotics 1 section 7:

Conventions, written once because every sign error in a robot comes from a convention that was assumed and not stated:

• Axes. The robot’s x axis points forward and its y axis to its left. The pose is (x, y, theta), with theta measured counter-clockwise from the world x axis, so a positive turn rate omega is a counter-clockwise turn (to the left), as in Robotics Fundamentals 4 section 3.
• Units. Metres, seconds, radians, metres per second. A “duty” is a signed fraction from −1 to +1 of the full battery voltage on the motor; a motor “command” is a signed fraction of the motor’s own top speed. Degrees appear only in printed output.
• Angles. A heading is wrapped to −π .. +π with wrap_pi, and a heading error is always wrapped after subtracting, so the robot turns the short way. But a total turn is not wrapped: turning twice on the spot is 4π (section 5 shows why a move needs that).
• Counts. The encoders give signed counts, positive when the wheel turns forward; a wheel that reads backwards is fixed once, in the hardware class, never in the maths (section 13 gives the bench test).

The first block of the module carries these conventions: the imports from the earlier chapters, and the two small helpers that every layer uses.

The try / except ImportError is there so that the file works in two settings: in one session, where the names from Programming for Robotics 4 already exist, and as a file next to filters.py, where they are imported. It is the only place where the module touches the earlier chapters at import time; the classes that follow take everything else as arguments.

2. The motor: dead zone, ramp, cap, brake and an always-safe stop

A motor command is a fraction from −1 to +1 of the motor’s own top speed. The Motor class turns it into a signed duty with four jobs, each a physical fact and each a few lines.

• Dead zone. Below some duty the motor does not turn at all (friction and the electrical resistance of the winding and driver must be overcome first; Robotics Fundamentals 3 section 2 has the motor model). A command of 0.05 is not 5 % of the way to anything. The map is duty = deadband + (1 − deadband) × |command|, so that the smallest command that is not zero turns the wheel and the full command is full duty. A command below a small cutoff means stop, not creep.
• Ramp. The command goes through the SlewLimiter of Programming for Robotics 4 section 3, which limits how fast it may change. A step from 0 to 1 then takes 1 / slew seconds, and a reversal from +1 to −1 takes 2 / slew: the wheels are never asked to reverse in one pass.
• Cap. A maximum duty, 1.0 normally and 0.25 for the first runs on a real robot (Robotics Fundamentals 10 section 7). The constructor refuses a cap that is below the dead zone: a robot that can never move is a bug to find at construction, not on the floor.
• Stop. stop() sets the command and duty to zero now, with no ramp and no dependence on any state: it works after a fault, in the middle of a ramp, with a bad value in the command, any number of times. The ramp is for driving. The way out must not be slowed by it.

Here is each in turn, with the numbers worked out first. With a dead zone of 0.12, a command of 0.5 gives 0.12 + 0.88 × 0.5 = 0.56. A ramp of 2.0 command units per second takes 1.0 / 2.0 = 0.5 s to reach full duty, which at 20 ms per pass is 25 passes; a reversal from +1 to −1 takes 2.0 / 2.0 = 1.0 s, that is 50 passes.

Three things in the output are worth reading. The command 0.01 gives a duty of exactly 0: below the cutoff of 0.02, the answer is “stop”, not a duty that would stall the motor and heat it. With a cap of 0.25 the fastest command that is not clipped is (0.25 − 0.12) / 0.88 = 0.1477, so the cap on a robot with this dead zone limits the speed to 15 % of its top speed, not to 25 %: a duty cap is not a speed cap, a point section 3 returns to. And the ramp’s measured time matches delta / slew to the pass. The ramp works on the command, so the duty jumps at the first pass from 0 to the dead zone plus a little (0.155 in the output): a ramp cannot soften that jump, which is physics, and it is the reason a very gentle ramp on a motor with a big dead zone still gives a visible jolt.

Brake or coast?

When the duty goes to zero, the driver either leaves the motor terminals open, so the motor free-wheels (coast), or shorts them together, so the motor’s own back-voltage drives a current that opposes the motion (brake). Both end at zero speed. They differ in how long it takes. To a good approximation the wheel speed falls like a first-order lag, v(t) = v₀ e^−t/τ, so the distance covered until it stops is the integral, v₀ τ: stopping distance = speed × time constant. The time constant is short for a brake (the driver does the work) and long for a coast (only friction does). The simulator’s own motor is the 0.08 s lag of Programming for Robotics 3 section 5. The two other values here are assumptions, in the order of magnitude a small geared motor gives, not measurements; measuring them is a bench test in section 13.

The simulated distances equal the predictions to the millimetre because the simulator’s motor is that same first-order lag; this proves the code and the formula agree, not that your motor behaves so. The lesson that survives is the ratio: with these assumed values, a coasting robot at 0.3 m/s rolls 75 mm, eight times as far as a braking one (9 mm), and the stop distance enters every safety budget (section 11). The class keeps this as a flag, brake, which DriveBase passes on to the hardware when both duties are zero: on this robot, braking is the better default for a stop, coasting the better choice at the end of a smooth move where a jerk is not wanted. The hardware class decides what “brake” means electrically (section 13).

3. Measuring the speed curve

The Motor class works in fractions of top speed, and top speed is something the software only believes (0.40 m/s in Config, copied from a design calculation). To drive in metres per second the robot needs the real map from duty to wheel speed, and the way to get it is the way Programming for Robotics 4 section 8 calibrated a sensor: apply known inputs, measure the outputs, fit a line. The input is a duty, the output is a wheel speed from the encoder counts, and the line is speed = k × (duty − deadband) above the dead zone. LinearCalibration.fit gives gain and offset for speed = gain × duty + offset; then k = gain and deadband = −offset / gain. Points where the wheel did not move are left out of the fit, because their speed (zero) is not on the line.

SpeedCurve is the open-loop model of one motor, and duty_for is its inverse: to get a speed, take the dead zone and add speed / k. RealMotors is a test rig: it wraps a simulator and makes the motors not what the software believes, with a dead zone and a gain per wheel, so that the sweep has something to find. It uses the idea that the same set_duty / ticks seam from Programming for Robotics 1 section 7 can sit between the program and a world that has been made to misbehave.

The sweep finds a dead zone of 0.1203 and a slope of 0.5231 m/s per unit duty, against the rig’s true 0.12 and 0.5227; the full-duty speed k (1 − deadband) = 0.4602 m/s is the 0.46 m/s the rig was given. The two points inside the dead zone are marked and left out. Then the practical point: for 0.30 m/s the measured curve asks for a duty of 0.6938 and the robot does 0.2998 m/s; the believed curve asks for 0.75 and the robot does 0.3294 m/s, 9.8 % too fast. That error is a wrong map, not noise, and nothing in a loop that never looks at the speed would find it. It also settles what the duty cap does: at a duty cap of 0.25 this robot cannot go faster than 0.0678 m/s. Robotics Fundamentals 10 section 7 predicted about 0.16 m/s at a 25 % cap for the build robot, from its own motor model, and the plain simulator gives 0.099 m/s (it takes duty 1.0 as 0.40 m/s with no dead zone). Three different answers to “how fast at 25 % duty?” mean that three different motors were assumed, and the real one has to be measured: the first floor run is the place to find out, with the wheels off the ground (section 13).

4. From a velocity command to wheel speeds

The controller of later chapters says “go forward at v and turn at omega”. A differential drive turns that into two wheel speeds with the inverse kinematics of Robotics Fundamentals 4 section 4: the left wheel goes at v − omega × track / 2 and the right wheel at v + omega × track / 2. Going the other way, the forward kinematics of its section 3 turn two measured wheel speeds into (v, omega). The code adds the one thing the formulas leave out: what to do when a wheel cannot go that fast.

If the faster wheel would need more than the motors can give, there are two answers. Clip that wheel, and the robot turns on a tighter curve than was asked for (the ratio of the wheel speeds has changed, and the turning radius is v / omega, the ratio of the two). Or scale both wheels by the same factor, and the radius survives: the robot goes slower along the same circle. The second is almost always what is wanted, and wheel_speeds returns the factor so that the caller (and the log) can see that the limit was hit. Geometry holds the three numbers that calibration will correct (metres per tick for each wheel, and the track width) in one place, so the corrected values can be dropped in without touching anything else.

A check with the numbers of Robotics Fundamentals 4 section 4 (track 0.150 m): a velocity command of (0.5 m/s, 3.0 rad/s) needs wheels at 0.275 and 0.725 m/s, and the turning radius is 0.5 / 3.0 = 0.1667 m. If the faster wheel is limited to 0.6 m/s, both are scaled by 0.6 / 0.725 = 0.8276, giving 0.2276 and 0.6 m/s, a forward speed of 0.4138 m/s, a turn rate of 2.4828 rad/s and the same 0.1667 m radius. Clipping only the fast wheel gives wheels of 0.275 and 0.6, v = 0.4375, omega = 2.1667 and a radius of 0.2019 m: a 21 % wider circle, which a robot in a corridor would feel as a wall.

5. Odometry: the pose from the encoder ticks

The pose (x, y, theta) is updated from the distance each wheel has rolled since the last update, d_L and d_R (ticks × metres per tick). The centre of the robot moves ds = (d_L + d_R) / 2 and the heading changes by dtheta = (d_R − d_L) / track (Robotics Fundamentals 4 section 5). What is left is where, in that step, the robot moved: along the old heading, along the new one, or along the arc it really drove. There are three choices and the code offers the last two.

• Old heading. x += ds cos(theta). Exact for a straight line, wrong for any turn: for large steps, very wrong.
• Midpoint. Use the heading half way through the step, theta + dtheta / 2. Cheap and nearly right.
• Arc. Drive a circle of radius R = ds / dtheta: x += R (sin(theta + dtheta) − sin(theta)), y −= R (cos(theta + dtheta) − cos(theta)). Exact when both wheels ran at constant speed during the step. It needs a guard for a straight line (dtheta near 0, where R is infinite), where it falls back on the midpoint formula, which is exact there.

The class takes unwrapped tick totals (the sum of the signed changes of the encoder counters, already free of the counter wrap-around of Programming for Robotics 4 section 9, which the SpeedEstimator keeps) and, besides the pose, keeps two running sums that moves will need: the distance driven and the total angle turned, which is not wrapped.

Robotics Fundamentals 4 section 5 had a worked example: 1200 ticks on the left wheel and 1500 on the right, in one deliberately long step, on the build robot (0.340 mm per tick, track 0.150 m). Running it through the class reproduces the numbers, and shows how far the two methods differ and why the loop period matters:

Four findings. First, the two methods differ by 8.8 mm in that one long step; the leading term of the difference, ds × dtheta² / 24, predicts 8.9 mm. Second, the second table shows why nobody notices at the usual loop period: at 20 ms the midpoint is 0.0002 mm from the arc, a thousandth of one encoder tick (0.340 mm); at 100 ms 0.03 mm; at 0.5 s 3.5 mm; at 2 s, with the robot turning 3 rad per step, the formula (225 mm) is already off from the true 201 mm because the heading change is not small any more. So: at a loop period of 20 ms either method is exact to well below the encoder’s resolution, and the choice matters only when something makes the loop slow, a point for the loop-statistics of Programming for Robotics 1 section 2. Third, the old-heading method puts the robot at (0.4595, 0) instead of (0.4248, 0.1504): 150 mm off in y in one step. Fourth, the last table is why turned_rad is not wrapped: theta comes back to −90 degrees, 0, 90 as the robot spins, which is right for a heading and useless for the question “how far have I turned?” asked by a move that was told to turn 720 degrees.

6. DriveBase: one pass of the loop

DriveBase puts the layers together. Its contract is small. The caller says set_velocity(v, omega) (metres per second and radians per second) and calls update(dt) once per pass of the loop of Programming for Robotics 1 section 4; the base reads the encoders, keeps the pose and the wheel speeds up to date, and sends duties to the motors. The order inside one pass is fixed and is the point of the class:

1. Sense. Read the ticks. A read that raises OSError is not a crash but a missing reading, kept as None (Programming for Robotics 2 section 11 for why buses fail). Otherwise update the two SpeedEstimators (the filtered wheel speeds) and the odometry.
2. Run the move, if there is one. A Move (section 7) is asked for its (v, omega), which counts as a command.
3. Check the guards (section 11). A guard may say hold (stand still for now: not latched) or fault (stand still until someone clears it: latched).
4. Drive or cut. With a fault or a hold, the motors are cut. Otherwise the wanted (v, omega) go through wheel_speeds with the limit of the slower wheel, each wheel’s speed is divided by its own top speed to give a command, and the two Motors turn that into duty.

The guard comes before the drive, and the cut is the same stop of section 2: nothing a guard needs from the rest of the pass can be missing, because it only reads what the sense step stored. A nice property follows: the pass can be tested without any hardware, with a list of scripted tick counts.

Five points about the code. set_velocity refuses garbage: a NaN or None for either number becomes (0, 0), because a NaN must never reach a motor. The motors’ commands are the wheel speeds divided by each wheel’s own measured top speed, so a motor that is weaker on one side is already compensated, once the curves of section 3 are given. max_wheel_mps is the speed of the slower wheel at the duty cap, so that scaling by wheel_speeds keeps the radius even when only one motor is the limit. A Move is a commander like any other: it calls set_velocity through the base, so the watchdog of section 11 sees it as a command. And stop() returns whether the hardware accepted the duty, and never raises.

Driving the base at six commands, each held for 3 s on the simulator of Programming for Robotics 3 (the plain simulator, whose motors are as believed), shows the whole pipeline at once:

Read it from left to right. The wanted wheel speeds are the kinematics of section 4; the duty is the wheel speed divided by 0.40 (the believed curve has no dead zone); the measured wheel speeds come from the encoders, and back through the forward kinematics they give (v, omega), which matches the command to about a tenth of a percent in the first four rows, and the true turn rate, which the simulator knows, matches the command too. The last two rows ask for more than the wheels can do: for (0.30, 2.0) the faster wheel would need 0.45 m/s against a limit of 0.4, so both wheels scale by 0.8889 and the robot goes at 0.2661 m/s and turns at 1.7759 rad/s, the same 0.15 m circle (0.30 / 2.0 = 0.15; 0.2661 / 1.7759 = 0.1499). For (0.40, 3.0), the scale is 0.64. The last line is the loop statistics from Programming for Robotics 1 section 2: a period of 20 ms and 0 overruns, because simulated time does not run late, which is also why the simulator tells nothing about timing on the real board.

7. Moves: open loop first, then closed loop

A move is a motion written as a step function, the style of Programming for Robotics 1 section 5: update(base, dt) is called once per pass, returns (v, omega) for this pass, never waits, and sets done on the pass that finishes it. Writing a move this way, instead of as a function that loops until it arrives, keeps the main loop in charge: the loop still checks the guards, feeds the watchdog and logs every pass while the move runs, and a move can be started, stopped or replaced between any two passes. A Sequence of moves is itself a move.

The trapezoid profile and an open-loop move

Driving a distance D at a top speed v_max with an acceleration a: speed up at a for t_acc = v_max / a, which covers d_acc = v_max² / (2a); cruise for (D − 2 d_acc) / v_max; brake at a. If D is less than 2 d_acc, the top speed is never reached and the profile is a triangle with the peak √(D a). An open-loop move follows that profile by the clock alone, with no encoders: it assumes the robot does what it is told. That is the simplest move and the baseline to measure the closed-loop one against.

For 1.0 m at up to 0.3 m/s with an acceleration of 0.6 m/s²: the ramp takes 0.5 s and covers 0.075 m, so the cruise is (1.0 − 0.15) / 0.3 = 2.833 s and the move takes 3.833 s. For 0.1 m the top speed would take 0.075 m twice, which does not fit, and the profile is a triangle with a peak of 0.2449 m/s.

The closed-loop move, and how to stop on a target

A closed-loop move measures its progress with the odometry and stops when the measurement says it has arrived. The hard part is the end. If the controller commands full speed until the distance to go is zero, the robot overshoots: the motors take about a time constant to react, and the robot carries momentum. The closed-loop moves therefore ask each pass for the speed from which the robot could still stop in the distance left. With constant deceleration a, the speed v from which one can stop in a distance r satisfies v² = 2 a r, so v = √(2 a r): the speed target falls as the square root of the remaining distance. Two refinements, both found by trying the plain version first:

• The lead. The motors lag, so the robot will still travel about v × lead after the command has gone (the stopping distance of section 2: lead = the motors’ time constant, plus the delays of the loop and the speed filter). The move takes that off the distance left: r = remaining − speed × lead_s. With lead_s = 0 the robot overshoots; the demonstration below sweeps it.
• The creep. Near the goal, √(2 a r) falls toward zero, and a motor with a dead zone does not turn for a tiny command. While the robot is still short of the target, the move never commands less than a small creep speed, so it arrives instead of stalling a few millimetres early. It finishes when it is within the tolerance and measured speed is below a settle threshold, and a time limit can turn a move that never arrives into failed = "timeout", a state the caller can see, not a robot that tries for ever.

DriveDistance adds an optional heading hold: omega = kp × (heading at start − heading now), a proportional controller on the angle (the P of the PID of Robotics Fundamentals 7, kept this simple on purpose until Programming for Robotics 6, planned and not yet written). TurnAngle does the same for an angle, using the total unwrapped turn of section 5, so that “turn 720 degrees” works. square() is four of each, in a Sequence.

A helper runs a move to its end and then keeps the loop going for a little longer, so the robot comes to rest before it is measured. It uses the same FixedRateLoop as the real program, with the simulator’s advance as its “sleep”, so the simulated world moves exactly when a real robot would have waited. The experiment drives 1 m six ways: an open-loop move and a closed-loop move on the plain simulator, and on the rig of section 3 (a robot whose motors are not what the software believes) an open-loop move with the believed curve, an open-loop move with the measured curve, and the closed-loop move with each:

The first two rows are the sanity check: on the plain simulator, both moves are within 0.3 mm. The next two are the lesson about open loops: on the motors of section 3, the open-loop move with the believed speed curve overshoots by 71.4 mm because the duty-to-speed map is wrong (it commands a duty that gives more speed than believed, for the 3.8 s the clock says); with the measured curve the same open-loop move is 0.3 mm short, which is as good as the closed loop and shows what a calibrated map is worth. The last two rows are the surprise, and the reason they are in the experiment. Closed loop with the believed curve does not overshoot: it stops 9.3 mm short and times out. The believed map has no dead zone, so near the goal, where the move asks for a creep speed of 0.015 m/s, the duty that results is below the motor’s real dead zone and the wheels do not turn. The loop is working perfectly and has nothing to push with; the creep floor is expressed in speed, and a speed can only be converted to a duty by a map that knows the dead zone. With the measured curve the closed loop arrives at −0.1 mm. The two lessons: a closed loop does not remove the need for the speed curve, and a move with a timeout is far better than one without, because the timeout turned an invisible stall into a visible “timeout”.

The lead, measured

The remaining parameter is lead_s. It stands for the distance the robot still rolls when the command goes (v × lead), and the best value should be about the motor’s time constant plus the speed filter’s delay: here 0.08 s plus a few passes. Instead of guessing, sweep it, and look at the worst final error over a set of distances and angles (5 distances, positive and negative, and 5 turns), and at what it costs in time:

With no lead the worst drive error is 10.7 mm and the worst turn 3.0 degrees. The error falls to a minimum of 0.46 mm and 0.36 degrees at lead_s = 0.20 and rises again at 0.30 (1.74 mm), which is the other side of the trade: the robot brakes too early and finishes on the creep speed. Both are inside the 3 mm tolerance; the lead tunes where inside it the robot stops. The price of the lead is time: 3.98 s for 1 m with none, 4.18 s with 0.20 s. The detail run shows the braking begin when the distance to go is 0.130 m against a prediction of v² / (2a) + v × lead = 0.075 + 0.060 = 0.135 m: the formula says where, to within 5 mm. The left-over error of the single run is below the 3 mm tolerance, and the move.error of 1.1 mm is odometry’s own view of it. The lead is a parameter to measure on the real robot, at its own speeds (section 13): this 0.20 s is a property of the simulated motor, 0.08 s lag plus the filter, not a constant of nature.

8. The square test, and a picture of the path

The standard test of a drive is a square: drive 1 m, turn 90 degrees, four times, and measure how far the robot ends from where it started. Odometry cannot be used to judge this test, since an odometry that is wrong about the robot will be wrong about its own end point too; the judge has to be outside, a tape measure on a real robot and the simulator’s true pose here. The helper start_frame_error expresses the end point in the start’s own frame (x ahead of where the robot began, y to its left), which is also how you would measure it on the floor; PathLog keeps the true path and the odometry’s together, and path_overlay draws both from above on a character grid, because a few numbers hide the shape of a failure that a picture shows at once.

First a square on the plain simulator, where the robot is exactly what the software believes. Then the case that matters, a robot that is not: a right tyre 1 % bigger than the software thinks and a true track of 0.155 m instead of 0.150 m, driven for a 2 m square with the heading hold off.

On the plain robot a 1 m square ends 7.7 mm from the start with a heading error of 0.58 degrees, in both directions; that is what the tolerances (3 mm and 0.01 rad per move) and the settling add up to over eight moves, and it is the best this method will do. On the robot that is not what the software believes, the end is 0.416 m ahead and 0.274 m to the right, with the robot facing 21.1 degrees off: a 2 m square misses by half a metre. The picture shows how. Odometry (the o path) believes it drew a perfect square and its end pose is (0.025, −0.025, 0.024), almost the start: the robot has no idea it is lost. The true path (*) is a spiral that opens, each side curving a little, each corner turning a little short. Nothing in the robot’s own data shows this, which is why the calibration of section 10 needs a measurement from outside.

9. Drift: where the errors come from, in code

Robotics Fundamentals 4 section 6 derived what a small error in the geometry does to a straight line and to a turn. Programming for Robotics 3 section 11 measured that on the simulator. This section repeats both with the real driving code, and adds what the drive code itself can and cannot fix. The first table checks the two worked examples of Robotics Fundamentals 4 section 6 against the code; the second shows what slip does; the third separates two kinds of fault that look alike at the end of a line and need different cures.

The first run: the right tyre is 1 % bigger, so the right wheel rolls 1 % further than the encoders (counting turns of the wheel) think, and the robot arcs left. It is driven 3 m “straight” with the heading hold on, and odometry still says (3.0001, 0.0000, 0.00): from the wheel counts the robot did go straight, and that is what the hold controls. The simulator’s truth is (2.9951, 0.3005, 11.46 degrees), identical to the formula of Robotics Fundamentals 4 section 6 (2.995, 0.300, 11.46 degrees) and to the rule of thumb of D² / 2 × (0.01 / 0.15) = 0.3000 m of sideways drift in 3 m. A heading hold built on odometry cannot repair an error that odometry does not see. The second: with a true track of 0.155 m and a believed track of 0.150 m, a “360 degree” spin by odometry is a real 348.6 degrees (Robotics Fundamentals 4 gave 348.4: the difference is where the controller stops); the ratio 0.9676 is 0.150 / 0.155.

The next question is what slip does, because a wheel that slips is a tyre whose effective size has shrunk, and so a wheel whose odometry over-counts. It was run with the heading hold on, at 2 m, which is the test 5 of Robotics Fundamentals 10 section 9.8:

A right tyre 1 % bigger gives 133.8 mm sideways and 7.64 degrees at 2 m, which is the “133 mm” of the test plan of Robotics Fundamentals 10 section 9.8. A right wheel that slips 2 % (scale 0.98) drifts the other way, 262.5 mm: the number is about twice as big as for 1 %, as expected (it scales with the size of the mismatch, here 2 % where the tyre was 1 %, and the sign flips). And if both wheels slip 2 %, the robot goes dead straight and ends 2 % short; no drift, no hint on the heading, and an odometry distance of 2.0002 m for a true x of 1.9602 m. A straight-line distance error is a scale error, and it needs a tape measure, not a heading measurement, to find it.

Two kinds of fault, and which fix works on which

A robot that curves has two possible causes that look the same on the floor. In one, the geometry is wrong (a bigger tyre, a wrong track): odometry itself is wrong, and neither a heading hold nor a better motor map can see it. In the other, the motors differ (different dead zones, a weaker side): odometry is right, it sees the robot curving, and the controller can correct it. The experiment drives a 2 m straight with each fault, with and without the hold, and with the believed or measured speed curves:

Reading the table. With different motors and no measured curves and no heading hold, the robot ends 791.4 mm off line and 52.75 degrees round: the drive is unusable. A heading hold alone brings that to −49.4 mm and −1.53 degrees, because odometry sees the curving and the controller corrects it. Per-wheel measured curves alone bring it to +2.6 mm and 0.16 degrees, because the cause is removed rather than corrected after the fact, and both together give +0.1 mm. With the bigger tyre, neither helps: 133.8 mm and 7.64 degrees whether the hold is on or off, since the hold is working from an odometry that is wrong the same way as the tyre. This is the dividing line of the whole chapter: faults that odometry can see are corrected by control; faults that odometry cannot see have to be measured, from outside, and put into the geometry. That is the next section.

10. Calibration: three measurements, three corrections

Section 9 left a list of numbers the software gets wrong and cannot see: the metres per tick of each wheel and the track width. They are measured from outside the robot, with a tape measure, in three steps, each correcting one thing, each needing no equipment beyond a floor, a tape and two squares of masking tape. The robot used is the one of section 8: a right tyre 1 % bigger and a true track of 0.155 m, neither of which the software knows.

Step 1: the scale of the straight line

Drive a long straight line, measure it with the tape, and compare it with what odometry says. The ratio multiplies both wheels’ metres per tick. It finds the mean of the two wheels’ scale errors, and nothing else: here (1.00 + 1.01) / 2 = 1.005, and the chord of an arc is shorter than the arc, which is why the measurement is 1.0043 and not 1.005.

Step 2: two squares, and what their end points say

A difference between the wheels, and a wrong track, cannot be found from a straight line and a spin alone, since each is contaminated by the other (the last line of the final demonstration of this section shows it). The method of Borenstein and Feng, as I remember it (I did not check the original; their paper is the place for details), is to drive the same square twice, once clockwise and once counter-clockwise, and to use the two end-point errors, because the two directions respond differently to the two kinds of error. The derivation that follows is mine, checked below in the simulator; the formulas were not copied.

Describe the robot by two small numbers. phi is the heading that each straight side adds, in radians (a wheel mismatch: the right wheel rolling further turns the robot to the left). psi is how much each corner turns beyond 90 degrees (a track error: a corner commanded as 90 degrees really turns 90 degrees × believed track / true track, which is less than 90 if the believed track is too small). For a counter-clockwise square, side k (k = 0 to 3) is driven at a heading k (90° + phi + psi) + phi / 2, since each earlier side and corner has added phi + psi + 90 degrees, and the side itself curves by phi, so on average it points phi / 2 further round. To first order in the small angles, a change e in the direction of a side of length L moves the end point of the square by L e at right angles to that side. Adding up the four sides, the sides at 0 and 180 degrees move the end point in y only, and the two at 90 and 270 degrees move it in x by −L e₁ and +L e₃. With e₁ = phi + psi + phi / 2 and e₃ = 3 (phi + psi) + phi / 2, the x error is L (e₃ − e₁) = 2 L (phi + psi). For the clockwise square each corner turns the other way, and the same sum gives −2 L (phi − psi). So the sum and the difference of the two measured x errors separate the two causes:

Then the corrections. A corner that turns 90 degrees + psi means (π / 2) × (believed track / true track) = π / 2 + psi, so the true track = believed track / (1 + 2 psi / π). The wheel mismatch: along one side of length L the right wheel rolls delta L further than the left, with phi = delta × L / track, so delta = phi × track / L; the two wheel scales are then 2 / (2 + delta) and 2 (1 + delta) / (2 + delta), whose ratio is 1 + delta and whose mean is 1, so that this step leaves the straight-line scale of step 1 alone. The first-order result is exact only for small angles, and the table below puts a number on that (it is the reason for the Newton steps in umbmark_fit): square_end_model is the exact end point from phi and psi, computed with the same pose_step as the robot.

For phi = 0.005 rad the first-order formula is within 1.3 % of the exact end point, for 0.02 rad within 5 %, but by phi = 0.13 rad it is out by 10 % on x_ccw and by 50 % on x_cw, and the first-order answer would have given phi = 0.1045 and psi = −0.0151 against the truth 0.13 and −0.05. The Newton iteration on the exact model recovers 0.1300 and −0.0500. This is not an artificial case: phi is the heading gained per side, and a right tyre 1 % bigger than the left gives 0.01 × 2 / 0.15 = 0.133 rad over a 2 m side, the mismatch that the test plan of Robotics Fundamentals 10 section 9.8 treats as typical. So for a 2 m square the exact model is needed; a shorter side or a smaller mismatch brings the first-order formula back into range.

Step 3: do it, and check it

The two squares, with the heading hold off (kp = 0) so that the drift shows (a hold would hide the very thing being measured), then the fit, then the corrected geometry:

The squares end at (−471.5, −952.1) mm clockwise and (+369.7, −251.4) mm counter-clockwise; only the two x values go into the fit. It finds phi = 0.1286 rad per side and psi = −0.0460 rad per corner (the corners turn short, as they should with a believed track that is too small), and the model with those two numbers reproduces the y errors that were not used in the fit: −251.4 mm (measured −251.4) and −953.5 mm (measured −952.1). That is the check that the two-parameter structure really describes this robot. The corrected geometry has a right-to-left ratio of 1.0099 (truth 1.0100) and a track of 154.52 mm (truth 155.00 mm). I did not chase the remaining 0.48 mm; the check below shows that it is small enough.

Now the check that counts: the same squares, now with the heading hold on as in real driving, before and after the correction, and the straight line and the spin of Robotics Fundamentals 10 section 9.8, before and after:

A 2 m square that ended 498 mm from the start (counter-clockwise) and 1013 mm (clockwise) now ends 1.3 mm and 4.7 mm from it, with a heading error of 0.18 and −0.03 degrees. The picture is the same square as in section 8, now closed. The 3 m straight line that drifted 290.9 mm sideways and ended 11.09 degrees round is now 16.0 mm and 0.09 degrees; the 360 degree spin that really turned 350.3 degrees now turns 359.4. The last line is the reason for two squares: the spin test alone, which is the method of Robotics Fundamentals 10 section 9.8, would have corrected the track to 154.25 mm, 0.75 mm short of the truth, because a spin on the spot is also moved by the difference between the wheels. In the simulator a single measurement can be a few hundredths of a millimetre off; on a real robot the tape and the floor give a few millimetres of spread, and the squares should be repeated and averaged. A tape error of 3 mm in each x value is 3 / 8000 = 0.0004 rad on phi and on psi for a 2 m square, which moves the wheel ratio by 0.003 % and the track by 0.04 mm; the square makes the measurement insensitive to the tape, and what limits real calibration is how repeatable the robot is on its floor.

11. Safe behaviour: cap, watchdog, guards and a stop that cannot fail

Robotics Fundamentals 10 section 7 sets rules for a robot that must not hurt itself or anyone; among them: speed-limit the first runs, treat the absence of a good signal as a fault, stop on silence. This section is those rules as code. The part of the class that matters most is not clever: each guard is a few lines, and its behaviour is a number you can measure.

The duty cap

The max_duty argument of DriveBase caps the duty, and with measured curves it also caps the speed that set_velocity can produce: max_wheel_mps is the speed of the slower wheel at the cap, and a command above it is scaled down along the same circle (section 4). It is a design choice that the cap is a property of the base, not of the controller: a control loop that has a bug, or wants 5 m/s, is still limited.

Watchdog, encoder guard and stall guard

• Command watchdog (not latching): if no set_velocity has been called for the timeout, the motors are zeroed, until one is called again. It catches a controller that has crashed or hung, while the rest of the program (and the loop) still runs. The watchdog on the board, the WDT of section 13, catches the case where the loop itself stops.
• Encoder guard (latching): the encoder reads fail (the Sensor / Health of Programming for Robotics 4 sections 2 and 10) or stop arriving. A counter that does not change is normal for a robot at rest, so “stuck” is switched off in this guard, and the next guard looks for “driven but not moving”.
• Stall guard (latching): a wheel told to move at least 0.04 m/s that measures under 10 % of what it was told, for 0.3 s, is stalled (a jammed wheel, or a broken encoder wire). Robotics Fundamentals 10 section 7 states the rule in terms of duty (above 30 % duty and under 10 % of the expected speed); this version works from the wanted speed, which is what the base knows at that point.

The difference between hold and fault is deliberate. A watchdog timeout is the controller being late or absent: when it speaks again, the base resumes. An encoder or stall fault means the robot’s knowledge of itself is no longer trustworthy: it latches, the motors stay off, new commands are ignored, and only clear_fault(), an explicit act by the program or the person, lets it go; even then the base does not move until a new command arrives. The first cause wins and stays on record.

The test rigs for faults come next: a wrapper that makes the encoders raise OSError from some time on (a dead bus), one that freezes one wheel’s tick count while the motors still run (a stalled wheel or a broken wire), and a small IdealHardware object with no simulator at all, used by the unit tests of section 12.

First the cap. Asking for 0.40 m/s from the plain simulator with the believed curve, and from the motor rig of section 3 with the measured curve, with the cap off and at 0.25:

With the cap at 0.25 the plain simulator gives 0.1000 m/s requested, 0.0991 measured; the motor rig, with its dead zone, gives 0.0678 m/s and a scale of 0.1696, meaning that the base scaled the command to 17 % of what was asked, along the same circle. Without the cap the rig’s measured curve needs only a duty of 0.885 for 0.40 m/s, because its top speed is 0.46. The cap is a ceiling on the duty; the speed it allows depends on the motors, so print it at start-up and read it.

Now the watchdog, and the numbers to design it with. The controller goes silent at t = 1.0 s, as a crash would, while the robot is driving at 0.3 m/s. After the timeout the base cuts the motors, and the robot then rolls on for about the motor’s time constant, so distance ≤ v × timeout + v × τ. With τ = 0.08 s (the simulator’s motor, an assumption on a real robot) this gives 0.3 × 0.25 + 0.3 × 0.08 = 0.099 m for a timeout of 0.25 s, and 0.054 m for 0.10 s. Robotics Fundamentals 10 section 7 computed 0.0525 m for its 100 ms watchdog, with a braking distance from a deceleration (0.0225 m) in place of the time constant (0.024 m): a different model of the same stop, and the same answer to within 2 mm.

Without the watchdog the robot drives on, 0.9 m further in the 3 s that follow, and never stops. With 0.25 s the motors are zeroed at 1.22 s and the robot rolls 0.090 m; with 0.10 s, at 1.06 s and 0.042 m. The measurements sit below the bounds (0.099 and 0.054), as they should, since the bound counts the whole of τ. Notice that the distance shown is measured from when the controller went quiet, not from when the watchdog fired: the timeout is part of the distance. Challenge 3 asks for the timeout that meets a stopping budget, and for the cost of choosing it too short.

The two guards for the robot’s own sensing, with the failures injected at t = 1.0 s while driving at 0.3 m/s:

A dead encoder bus is noticed in 0.08 s (5 failed reads at 20 ms and a little more: bad_max = 5) and the fault reads ‘encoder left bad’, with the robot 0.275 m from the start and the duty at zero; it then coasts for the 24 mm of section 2 to a final x of 0.299 m. A frozen wheel needs the 0.3 s of the stall rule and a little more (the speed estimate has to fall under the threshold first) and is noticed at 1.42 s, naming the wheel, after 0.377 m; the extra 0.12 s beyond the 0.3 s is, as I read the code, the time the filtered wheel speed needs to fall under the threshold, and I did not trace it further. The last three lines are the latch: with the fault set, a new command gives duty 0; after clear_fault(), still 0 (no command yet); and only after a new command do the wheels move (duty 0.080 for a command of 0.3 m/s, which at this stage is a ramp, not the final duty). The robot is not allowed to restart by itself after a fault.

A stop that does not raise

The last piece is the stop itself. stop() cancels the move, forgets the command, zeroes the motors, and tells the caller whether the hardware took the duty, instead of raising an exception. A safety path that can throw is a safety path that can be skipped by the exception handler above it. Here the driver of the motors stops answering exactly when the stop is called, twice:

The first two calls return False, count the errors and do not raise, while the simulated robot is still driving at 0.3 m/s (the duty it had is still in the driver, and the robot moved 0.030 m, then 0.060 m, in the 0.1 s after each call); the third gets through, the wheels’ command goes to zero and the robot has moved 0.077 m since the first call. A caller that gets False can retry, switch off the driver’s standby pin, or cut the power, as the E-stop of Robotics Fundamentals 10 section 7 does by hardware: the point is that the failure is reported.

12. Tests

The module has seventeen tests, named test_drive_..., run by the run_tests of Programming for Robotics 4 section 11, which takes a dictionary of test functions, runs them and lists the failures. They are written the way that chapter taught: small, deterministic, one fact each, with the plain simulator or the hardware-free IdealHardware as the world.

All seventeen pass. Two of the tests are worth a comment, because they are the kind that stay useful. test_drive_odometry_worked_example pins the code to a number written in another chapter, so that a change to the update formula that breaks Robotics Fundamentals 4 section 5 is noticed at once. test_drive_umbmark_recovers_known_errors goes the other way: it builds a robot from known phi and psi, produces its end points with the exact model, and checks that the fit finds them again: a test of a derivation, not only of code. One honest limit: the tests were not themselves tested by breaking the code on purpose (a “mutation test”), so I cannot say how many mistakes they would miss.

13. Running it on the real robot: notes and a bench-test list

The module needs a hardware object with ticks(), set_duty(left, right) and a clock, and optionally set_brake(on). The PicoHardware of Programming for Robotics 2 section 10 has the first two, with encoders counted by interrupts, but its motor channel applies an assumed dead zone of its own (command_to_duty of Programming for Robotics 2 section 2): Motor now owns the dead zone, and using both would count it twice, so the subclass below replaces the channel by one that applies no dead zone and takes a duty from −1 to +1. The pins are the build robot’s (GP2 and GP3 for the left encoder, GP4 and GP5 for the right, GP9 for the driver’s STBY, GP10 to GP12 for the left motor, GP13 to GP15 for the right), the PWM frequency is 20 kHz, above hearing.

For a TB6612-type driver, as I remember the data sheet: with IN1 high and IN2 low the motor turns one way, with the inputs swapped the other way, with both low the output is off and the motor coasts, with both high the terminals are shorted and the motor brakes, and STBY must be high for any of it to work. That is why brake() raises STBY and PicoDrive.set_duty lowers it only when both duties are zero. The main program then builds the drive, adds the guards, wraps the loop in a board watchdog and always ends with a stop:

Things to notice: the duty cap of 0.25 and the slew of 4 command units per second for the first run; the three guards; the board WDT, which is fed once per pass and resets the board, and with it the pins, if the loop stops; step raising SystemExit on a fault, after the base has already cut the motors; and the finally, which runs base.stop() whatever happened.

The bench-test list, in order

Do these in this order, each one before the next, as Robotics Fundamentals 10 section 7 and its test plan (section 9.8) say, and write down every number: they are what the code needs.

1. Wheels off the ground, robot on a block, a current-limited supply if you have one. Nothing here moves the robot.
2. Direction. Send a small positive duty to each wheel in turn (hw.set_duty(0.3, 0)): both must turn forward. A wheel that turns the wrong way is fixed in the hardware class (swap the two direction pins in the pin map), not by a minus sign in the maths.
3. Encoder sign. Turn each wheel forward by hand: its count must increase. If it falls, swap that encoder’s A and B pins in the pin map. Do this before any motor test: a feedback sign that is wrong makes a controller run away.
4. Ticks per revolution. Mark a wheel, turn it exactly ten times by hand, divide the count by ten: the build robot should give about 600 (Robotics Fundamentals 10 section 9.8, the encoder test). Convert to metres per tick with the measured wheel diameter; step 1 of section 10 then checks the result against a tape.
5. Brake and coast. Run the wheels in the air at a duty of 0.25, cut the power, and time how long each wheel keeps turning with the driver off (coast) and with the brake. This gives the two time constants that section 2 assumed; take the time for the speed to fall to 37 % of its start as τ.
6. The speed curve (section 3) on the floor, at the 0.25 cap and then in steps up, with a tape measure over a known distance; fit the deadband and the slope, left and right, and give DriveBase the curves.
7. The lead. Run the sweep of section 7 at the speeds you will use: the best lead_s is about the time constant of the real motor plus the delays of the loop.
8. The three calibration steps of section 10, with the heading hold off for the squares, then check with it on. Repeat and average.
9. Watchdog and guards. Pull the encoder wire with the motors running (wheels in the air), stop the controller on purpose, and confirm each guard acts as the simulator showed, with the distance you measured. Only then raise the cap in steps.

14. The reference listing: drive.py

Here is the module in one piece: the blocks of this chapter without an output, joined in the order of the page. It needs filters.py of Programming for Robotics 4 next to it for its imports; the demonstrations also need the simulator. A __main__ at the end runs the seventeen tests.

15. Challenges

Each challenge has a worked solution, checked by running it, in solutions/roboprog1-5_exerciseN.txt. All three need the module of section 14 (the blocks of the chapter without an output) and the simulator, in one session.

Next: Programming for Robotics 6, Control Loops in Code

This chapter gave the robot a body that obeys: a velocity command in, wheel speeds, duties and a pose out, moves that stop where they are told, calibration numbers measured against a tape, and guards that cut the motors when something is wrong. The controllers inside it are still the plainest ones: a square-root speed profile and a single proportional gain for the heading. Programming for Robotics 6 (planned; not yet written) is about the control loops themselves, in code: the proportional, integral and derivative controller of Robotics Fundamentals 7 as a small class with anti-windup and a derivative that does not amplify noise, a speed loop around the motors of this chapter that makes the measured wheel speed follow the command, a position and heading loop on the odometry, tuning against the simulator by measurement instead of by feel, and the test that tells a good tuning from a lucky one.