Programming for Robotics 1, Chapter 4: Challenge 2 solution (a counter that wraps, and a loop that stalls)

Idea: the modular difference of section 9 maps the change of an N-bit counter into -2^(N-1) .. 2^(N-1) - 1, so it is right only while the true change between two readings is under 2^(N-1) ticks. For 12 bits that is 2048 ticks, 2048 x 0.34034 mm = 0.697 m of travel, and at the top speed of 0.4 m/s the counter must be read at least every 0.697 / 0.4 = 1.743 s. (b) The test is the estimator on a normal loop, with the true distance from the simulator for comparison. (c) A loop that stalls is a loop whose interval is long: the simulator is advanced by the whole interval in one call and the estimator is updated once, as a stalled real loop would.

CODE:
mpt = Config.METRES_PER_TICK
bits = 12
print("(a) 12 bits: the counter holds %d ticks; the change between two readings must stay under %d ticks = %.4f m"
      % (1 << bits, 1 << (bits - 1), (1 << (bits - 1)) * mpt))
print("    at 0.4 m/s that is read-at-least-every %.3f s" % ((1 << (bits - 1)) * mpt / 0.4))

print("(b) 3 m at duty 0.5, a 20 ms loop")
sim = Simulator(World(6.0, 2.0), seed=1, pose=(0.3, 1.0, 0.0))
hw = WrappingTicks(sim, bits)
est = SpeedEstimator(mpt, window=5, alpha=0.5, bits=bits)
est.update(hw.ticks()[0], sim.now_s())
sim.set_duty(0.5, 0.5)
while sim.pose()[0] - 0.3 < 3.0:
    sim.advance(0.02)
    est.update(hw.ticks()[0], sim.now_s())
true_m = sim.pose()[0] - 0.3
print("    true distance %.4f m (%d ticks); counter reads %d; wrapped %d times" % (true_m, sim.ticks()[0], hw.ticks()[0], sim.ticks()[0] // (1 << bits)))
print("    estimated distance %.4f m (error %+.2f mm), speed estimate %.3f m/s (true %.3f)"
      % (est.distance_m, (est.distance_m - true_m) * 1000, est.speed_mps, sim.model.true_speeds_mps()[0]))

print("(c) full speed, the loop only runs every T seconds, for about 8 s")
for interval in (1.5, 2.0):
    sim = Simulator(World(6.0, 2.0), seed=1, pose=(0.3, 1.0, 0.0))
    hw = WrappingTicks(sim, bits)
    est = SpeedEstimator(mpt, window=1, bits=bits)
    est.update(hw.ticks()[0], sim.now_s())
    sim.set_duty(1.0, 1.0)
    first_step = None
    for _ in range(int(round(8.0 / interval))):
        sim.advance(interval)
        before = est.total
        est.update(hw.ticks()[0], sim.now_s())
        if first_step is None:
            first_step = (est.total - before, sim.ticks()[0])
    print("    T = %.1f s: true %.3f m; estimated %.3f m; first interval: true %d ticks, counted %d" %
          (interval, sim.pose()[0] - 0.3, est.distance_m, first_step[1], first_step[0]))
END CODE
OUTPUT:
(a) 12 bits: the counter holds 4096 ticks; the change between two readings must stay under 2048 ticks = 0.6970 m
    at 0.4 m/s that is read-at-least-every 1.743 s
(b) 3 m at duty 0.5, a 20 ms loop
    true distance 3.0040 m (8826 ticks); counter reads 634; wrapped 2 times
    estimated distance 3.0038 m (error -0.17 mm), speed estimate 0.200 m/s (true 0.200)
(c) full speed, the loop only runs every T seconds, for about 8 s
    T = 1.5 s: true 2.968 m; estimated 2.968 m; first interval: true 1668 ticks, counted 1668
    T = 2.0 s: true 3.168 m; estimated -2.408 m; first interval: true 2256 ticks, counted -1840
END OUTPUT

Reading the output. (a) 2048 ticks, 0.6970 m, 1.743 s at 0.4 m/s. (b) The robot drove 3.0040 m (8826 ticks), the 12-bit counter reads 634 (8826 - 2 x 4096) and has wrapped twice; the estimator, with a window of 5 passes and an exponential filter, says 3.0038 m: the error of 0.17 mm is less than one tick (0.34 mm), and the speed estimate is 0.200 m/s, the true speed. (c) At T = 1.5 s the robot covers about 0.6 m per interval, 1668 ticks in the first one (the motors are still speeding up), under the limit of 2048, and the estimator is exactly right (2.968 m). At T = 2.0 s the first interval is 2256 ticks, over the limit: the counter has gone 2256 ticks forward, which the modular difference reads as 2256 - 4096 = -1840 ticks, a robot driving backwards for 0.63 m in two seconds. Every later interval has the same fate, and after four of them the estimate is -2.408 m against a true +3.168 m. Nothing in the data says that the count was wrong: the modular difference never fails loudly. The limit has to be kept by the loop period.

Check on method: the three ways to get this wrong are (1) taking the limit as 2^N instead of 2^(N-1): a change of 3000 ticks of a 12-bit counter is indistinguishable from -1096, and the half range is where the two meet, (2) testing only the normal loop, in which case the estimator looks perfect (part (b)), when the whole point of the limit is what happens when the loop stalls (a garbage collection, a blocking read, a debugger breakpoint), and (3) forgetting that the limit depends on the speed: a robot that is safe at 0.2 m/s is not at 0.4 m/s. A real driver can protect itself: if a tick count changes by more than a plausible maximum from the speed limit, report a fault and not a number.