Programming for Robotics 1, Chapter 1: Challenge 3 solution (a stall check through the hardware abstraction)

The rule (Robotics Fundamentals 10 section 7): duty above 30 %, and wheel speed under 10 % of the speed the duty asks for, held for 0.3 s: cut the motors.

Design points:
  - Everything is taken from the Robot's own state, through the three hardware methods that already exist. The check never reads hw.duty (that is an attribute of the fakes, not part of the interface), so it works unchanged on real hardware.
  - The speed measured on a pass was produced by the command of the PREVIOUS pass, so the check compares the measured speed with the previous command (state.cmd_left_mps before the update), not with the one just issued.
  - A timer of stalled time: add dt while the condition holds, reset to zero when it does not (the same "in a row" idea as the debounce of Challenge 2).
  - Latch: once the stall is declared, update() only stops the motors, and the fault stays "stall".
  - 0.3 s is 15 passes of 0.02 s, but 15 additions of 0.02 in floating point may give 0.29999999999999993, so the comparison has a tiny tolerance (1e-9).
  - Timing expected: pass 1 issues the first command; pass 2 is the first pass that judges it (wanted 0.3 m/s, measured 0); the timer reaches 15 x 0.02 = 0.30 s on pass 16.

CODE:
class StallRobot(Robot):
    """Robot plus the stall rule of Robotics Fundamentals 10 section 7, latched."""

    STALL_DUTY = 0.30                  # only judge a stall when the motors are driven harder than this
    STALL_SPEED_FRACTION = 0.10        # stalled: slower than this fraction of the speed that was asked for
    STALL_TIME_S = 0.30                # ... for this long

    def __init__(self, hw, cfg=Config):
        Robot.__init__(self, hw, cfg)
        self.stall_s = 0.0
        self.latched = False

    def update(self, dt_s):
        if self.latched:
            self.stop()                # once latched, never drive again
            return
        asked_mps = abs(self.state.cmd_left_mps)       # the command of the PREVIOUS pass: it made the motion just measured
        Robot.update(self, dt_s)
        asked_duty = asked_mps / self.cfg.MAX_SPEED_MPS
        stalled = (asked_duty > self.STALL_DUTY
                   and abs(self.state.speed_mps) < self.STALL_SPEED_FRACTION * asked_mps)
        if stalled:
            self.stall_s += dt_s
        else:
            self.stall_s = 0.0
        if self.stall_s >= self.STALL_TIME_S - 1e-9:   # the tolerance absorbs 15 x 0.02 not being exactly 0.3
            self.latched = True
            self.state.fault = "stall"
            self.stop()


class StuckHardware(WallWorld):
    """Jammed wheels: the motors are driven, but the encoders never count."""

    def advance(self, dt_s):
        pass


def passes_until_latched(hw, max_passes):
    robot = StallRobot(hw)
    for k in range(1, max_passes + 1):
        hw.advance(0.02)
        robot.update(0.02)
        if robot.latched:
            return k, robot
    return None, robot


stuck = StuckHardware(1.0)
k, robot = passes_until_latched(stuck, 40)
print("stuck wheels: stall latched on pass %d (t = %.2f s), fault = %r, duty = %s" % (k, robot.state.t_s, robot.state.fault, stuck.duty))
for _ in range(5):
    robot.update(0.02)
print("five passes later: duty = %s, still latched = %s" % (stuck.duty, robot.latched))

free = WallWorld(1.0)
k, robot = passes_until_latched(free, 500)
print("free wheels: latched = %s, fault = %r, final range = %.3f m" % (k is not None, robot.state.fault, free.range_m()))
END CODE
OUTPUT:
stuck wheels: stall latched on pass 16 (t = 0.32 s), fault = 'stall', duty = (0.0, 0.0)
five passes later: duty = (0.0, 0.0), still latched = True
free wheels: latched = False, fault = None, final range = 0.250 m
END OUTPUT

Reading the output: with jammed wheels the robot declares the stall on pass 16, 0.32 s after it starts (0.30 s of stall plus the first pass that issued the command), and stays stopped. Compare the 1 A fuse of Robotics Fundamentals 10 section 7, which takes about a second to open: the software rule acts first, and the fuse is the backup. On the free-running WallWorld the rule never fires, including during the slow approach to the stop line, where the duty falls below 30 % and the check switches itself off.

Check on method: a common slip is to compare against the NEW command (the stall then looks like it starts one pass early), or to forget the reset, so that a robot that merely slows for a moment accumulates "stall time" over a whole run and trips later for no reason. Another is to drop the latch: the robot would restart, stall again, restart ... and cook the driver in a stutter.